//
// Created by Administrator on 2019\11\1 0001.
//
#include "stereo_utils.h"
#include "native_debug.h"
#include <iostream>

void r3d_depth_image::VisualizeOpticalFlow(
        cv::Mat & output,
        const std::vector<cv::Point2f> & src,
        const std::vector<cv::Point2f> & dst) {
    float dx, dy, angle;
    if (src.size() != dst.size()) {
        return;
    }
    for (int i = 0; i<dst.size(); i++) {
        dy = dst[i].y - src[i].y;
        dx = dst[i].x - src[i].x;
        angle = atan(dy / (1.0 + dx)) / 3.1415926 * 180;
        if (angle < 0) {
            cv::circle(output, src[i], -angle / 2, cv::Scalar(0, 100, 200), 1);
            cv::arrowedLine(output, src[i], dst[i], cv::Scalar(100, 200, 0), 1, 8, 0);
        }
        else {
            cv::circle(output, src[i], angle / 2, cv::Scalar(0, 200, 100), 1);
            cv::arrowedLine(output, src[i], dst[i], cv::Scalar(200, 100, 0), 1, 8, 0);
        }
    }
}


// To use SIMD128, keep operation restricted in only [+-*/]
// Suppose depth of neighbors: 255-{v1,v2,v3,v4};
// Suppose color distance of neighbors: 255-{d1,d2,d3,d4}
// s = v1^k + v2^k + v3^k + v4^k(k = 1, 2,...)
// polarize d_{i} to be 0 or 1:
// t = (d1 + d2 + d3 + d4)/4;
// d_{i} = d_{i} > t
// t = d1 + d2 + d3 + d4
// v = v1*(v1^{k}/s*d1/t) +
//     v2*(v2^{k}/s*d2/t) +
//     v3*(v3^{k}/s*d3/t) +
//     v4*(v4^{k}/s*d4/t);
void r3d_depth_image::Erode_v3(
        cv::Mat & o_,
        const cv::Mat & i_,
        const cv::Mat & rgb,
        int r,
        uint8_t thr_
) {
    if (i_.type() != CV_8UC1 ||
        rgb.type() != CV_8UC3 ||
        i_.size() != rgb.size()) {
       return;
    }
    if (o_.size() != i_.size() || o_.type() != i_.type()) {
        o_.create(i_.size(), i_.type());
    }

    unsigned char * p_o = o_.data;
    unsigned char * p_i = i_.data;
    const unsigned char * p_c = rgb.data;

    int h, w;
    h = i_.size().height;
    w = i_.size().width;
    unsigned char v_o; // value of depth at the origin
    unsigned char v[4]; // depth of neighbors
    unsigned char c[3]; // color of origin
    unsigned short d_u16[4]; // // color distance in u16
    unsigned char d_u8[4]; // color distance in u8
    unsigned char d_mean_u8; // mean color distance in u8
    unsigned short sum_u16; // sum of weights in u16
    unsigned short v_u16; // the weighted sum of depth
    int addr_[4], addr_0;
    unsigned char zero_flag; // the flag for sum being zero or not

    // use non-uniform weighted sum to replace min/max
    for (int i = 0; i < h; ++i) {
        for (int j = 0; j < w; ++j) {
            // origin
            addr_0 = i * w + j;
            v_o = p_i[addr_0];
            addr_0 *= 3;
            c[0] = p_c[addr_0];
            c[1] = p_c[addr_0 + 1];
            c[2] = p_c[addr_0 + 2];
            // neighbors
            zero_flag = i >= r;
            addr_[0] = zero_flag * (i - r) * w + j;
            zero_flag = i + r < h;
            addr_[1] = (zero_flag * (i + r) + (1 - zero_flag) * (h - 1)) * w + j;
            zero_flag = j >= r;
            addr_[2] = i * w + zero_flag * (j - r);
            zero_flag = j + r < w;
            addr_[3] = i * w + zero_flag * (j + r) + (1 - zero_flag) * (w - 1);
            v[0] = p_i[addr_[0]];
            v[1] = p_i[addr_[1]];
            v[2] = p_i[addr_[2]];
            v[3] = p_i[addr_[3]];
            // channel R
            addr_[0] *= 3;
            addr_[1] *= 3;
            addr_[2] *= 3;
            addr_[3] *= 3;
            d_u16[0] = std::abs(p_c[addr_[0]] - c[0]);
            d_u16[1] = std::abs(p_c[addr_[1]] - c[0]);
            d_u16[2] = std::abs(p_c[addr_[2]] - c[0]);
            d_u16[3] = std::abs(p_c[addr_[3]] - c[0]);
            // channel G
            addr_[0] ++;
            addr_[1] ++;
            addr_[2] ++;
            addr_[3] ++;
            d_u16[0] += std::abs(p_c[addr_[0]] - c[1]);
            d_u16[1] += std::abs(p_c[addr_[1]] - c[1]);
            d_u16[2] += std::abs(p_c[addr_[2]] - c[1]);
            d_u16[3] += std::abs(p_c[addr_[3]] - c[1]);
            // channel B
            addr_[0] ++;
            addr_[1] ++;
            addr_[2] ++;
            addr_[3] ++;
            d_u16[0] += std::abs(p_c[addr_[0]] - c[2]);
            d_u16[1] += std::abs(p_c[addr_[1]] - c[2]);
            d_u16[2] += std::abs(p_c[addr_[2]] - c[2]);
            d_u16[3] += std::abs(p_c[addr_[3]] - c[2]);
            // normalize color distance to uint8
            d_u8[0] = d_u16[0] >> 2;
            d_u8[1] = d_u16[1] >> 2;
            d_u8[2] = d_u16[2] >> 2;
            d_u8[3] = d_u16[3] >> 2;
            // get average of color distances
            sum_u16 = d_u8[0];
            sum_u16 += d_u8[1];
            sum_u16 += d_u8[2];
            sum_u16 += d_u8[3];
            d_mean_u8 = sum_u16 >> 2;
            d_mean_u8 = d_mean_u8 < thr_ ? d_mean_u8 : thr_;
            // polarize color distances
            d_u8[0] = d_u8[0] <= d_mean_u8;
            d_u8[1] = d_u8[1] <= d_mean_u8;
            d_u8[2] = d_u8[2] <= d_mean_u8;
            d_u8[3] = d_u8[3] <= d_mean_u8;
            // get average of depth
            sum_u16 = v[0];
            sum_u16 += v[1];
            sum_u16 += v[2];
            sum_u16 += v[3];
            d_mean_u8 = sum_u16 >> 2;
            // convert from depth to weight[only on erode]
            d_u16[0] = v[0] <= d_mean_u8;
            d_u16[1] = v[1] <= d_mean_u8;
            d_u16[2] = v[2] <= d_mean_u8;
            d_u16[3] = v[3] <= d_mean_u8;
            // compute the weight for each neighbor
            d_u16[0] &= d_u8[0];
            d_u16[1] &= d_u8[1];
            d_u16[2] &= d_u8[2];
            d_u16[3] &= d_u8[3];
            // sum of weights
            sum_u16 = d_u16[0];
            sum_u16 += d_u16[1];
            sum_u16 += d_u16[2];
            sum_u16 += d_u16[3];
            zero_flag = sum_u16 == 0;
            // get the final weighted depth
            v_u16 = v[0] * d_u16[0];
            v_u16 += v[1] * d_u16[1];
            v_u16 += v[2] * d_u16[2];
            v_u16 += v[3] * d_u16[3];
            sum_u16 += zero_flag;
            v_u16 /= sum_u16;
            v_u16 *= (!zero_flag);
            v_o *= zero_flag;
            v_u16 += v_o;
            p_o[i * w + j] = v_u16;
        }
    }
}


void r3d_depth_image::Dilate_v3(
        cv::Mat & o_,
        const cv::Mat & i_,
        const cv::Mat & rgb,
        int r,
        uint8_t thr_
) {
    if (i_.type() != CV_8UC1 ||
        rgb.type() != CV_8UC3 ||
        i_.size() != rgb.size()) {
		return;
    }
    if (o_.size() != i_.size() || o_.type() != i_.type()) {
        o_.create(i_.size(), i_.type());
    }

    unsigned char * p_o = o_.data;
    unsigned char * p_i = i_.data;
    const unsigned char * p_c = rgb.data;

    int h, w;
    h = i_.size().height;
    w = i_.size().width;
    unsigned char v_o; // value of depth at the origin
    unsigned char v[4]; // depth of neighbors
    unsigned char c[3]; // color of origin
    unsigned short d_u16[4]; // // color distance in u16
    unsigned char d_u8[4]; // color distance in u8
    unsigned char d_mean_u8; // mean color distance in u8
    unsigned short sum_u16; // sum of weights in u16
    unsigned short v_u16; // the weighted sum of depth
    int addr_[4], addr_0;
    unsigned char zero_flag; // the flag for sum being zero or not

    // use non-uniform weighted sum to replace min/max
    for (int i = 0; i < h; ++i) {
        for (int j = 0; j < w; ++j) {
            // origin
            addr_0 = i * w + j;
            v_o = p_i[addr_0];
            addr_0 *= 3;
            c[0] = p_c[addr_0];
            c[1] = p_c[addr_0 + 1];
            c[2] = p_c[addr_0 + 2];
            // neighbors
            zero_flag = i >= r;
            addr_[0] = zero_flag * (i - r) * w + j;
            zero_flag = i + r < h;
            addr_[1] = (zero_flag * (i + r) + (1 - zero_flag) * (h - 1)) * w + j;
            zero_flag = j >= r;
            addr_[2] = i * w + zero_flag * (j - r);
            zero_flag = j + r < w;
            addr_[3] = i * w + zero_flag * (j + r) + (1 - zero_flag) * (w - 1);
            v[0] = p_i[addr_[0]];
            v[1] = p_i[addr_[1]];
            v[2] = p_i[addr_[2]];
            v[3] = p_i[addr_[3]];
            // channel R
            addr_[0] *= 3;
            addr_[1] *= 3;
            addr_[2] *= 3;
            addr_[3] *= 3;
            d_u16[0] = std::abs(p_c[addr_[0]] - c[0]);
            d_u16[1] = std::abs(p_c[addr_[1]] - c[0]);
            d_u16[2] = std::abs(p_c[addr_[2]] - c[0]);
            d_u16[3] = std::abs(p_c[addr_[3]] - c[0]);
            // channel G
            addr_[0] ++;
            addr_[1] ++;
            addr_[2] ++;
            addr_[3] ++;
            d_u16[0] += std::abs(p_c[addr_[0]] - c[1]);
            d_u16[1] += std::abs(p_c[addr_[1]] - c[1]);
            d_u16[2] += std::abs(p_c[addr_[2]] - c[1]);
            d_u16[3] += std::abs(p_c[addr_[3]] - c[1]);
            // channel B
            addr_[0] ++;
            addr_[1] ++;
            addr_[2] ++;
            addr_[3] ++;
            d_u16[0] += std::abs(p_c[addr_[0]] - c[2]);
            d_u16[1] += std::abs(p_c[addr_[1]] - c[2]);
            d_u16[2] += std::abs(p_c[addr_[2]] - c[2]);
            d_u16[3] += std::abs(p_c[addr_[3]] - c[2]);
            // normalize color distance to uint8
            d_u8[0] = d_u16[0] >> 2;
            d_u8[1] = d_u16[1] >> 2;
            d_u8[2] = d_u16[2] >> 2;
            d_u8[3] = d_u16[3] >> 2;
            // get average of color distances
            sum_u16 = d_u8[0];
            sum_u16 += d_u8[1];
            sum_u16 += d_u8[2];
            sum_u16 += d_u8[3];
            d_mean_u8 = sum_u16 >> 2;
            d_mean_u8 = d_mean_u8 < thr_ ? d_mean_u8 : thr_;
            // polarize color distances
            d_u8[0] = d_u8[0] <= d_mean_u8;
            d_u8[1] = d_u8[1] <= d_mean_u8;
            d_u8[2] = d_u8[2] <= d_mean_u8;
            d_u8[3] = d_u8[3] <= d_mean_u8;
            // get average of depth
            sum_u16 = v[0];
            sum_u16 += v[1];
            sum_u16 += v[2];
            sum_u16 += v[3];
            d_mean_u8 = sum_u16 >> 2;
            // convert from depth to weight[only on dilate]
            d_u16[0] = v[0] >= d_mean_u8;
            d_u16[1] = v[1] >= d_mean_u8;
            d_u16[2] = v[2] >= d_mean_u8;
            d_u16[3] = v[3] >= d_mean_u8;
            // compute the weight for each neighbor
            d_u16[0] &= d_u8[0];
            d_u16[1] &= d_u8[1];
            d_u16[2] &= d_u8[2];
            d_u16[3] &= d_u8[3];
            // sum of weights
            sum_u16 = d_u16[0];
            sum_u16 += d_u16[1];
            sum_u16 += d_u16[2];
            sum_u16 += d_u16[3];
            zero_flag = sum_u16 == 0;
            // get the final weighted depth
            v_u16 = v[0] * d_u16[0];
            v_u16 += v[1] * d_u16[1];
            v_u16 += v[2] * d_u16[2];
            v_u16 += v[3] * d_u16[3];
            sum_u16 += zero_flag;
            v_u16 /= sum_u16;
            v_u16 *= (!zero_flag);
            v_o *= zero_flag;
            v_u16 += v_o;
            p_o[i * w + j] = v_u16;
        }
    }
}


void r3d_depth_image::SmoothDisparity(cv::Mat & o_,
                     const cv::Mat & i_,
                     const cv::Mat & edge,
                     bool use_mask,
                     int min_support
) {
    if (i_.type() != CV_8UC1) {
		return;
    }

    if (edge.type() != CV_8UC1) {
		return;
    }

    int h, w;
    h = i_.size().height;
    w = i_.size().width;
    if (o_.size() != i_.size() || o_.type() != i_.type()) {
        o_.create(i_.size(), i_.type());
    }

    unsigned char * data_o = o_.data;
    const unsigned char * data_i = i_.data;
    const unsigned char * data_edge = edge.data;
    unsigned char v;

    // create a label matrix to store labels for every pixel in main view
    cv::Mat id_map = cv::Mat::zeros(i_.size(), CV_16UC1);
    unsigned short * data_id = (unsigned short *)(id_map.data);
    std::vector<cv::Point2i> nodes;
    nodes.reserve(h * w / 4);
    nodes.resize(0);
    std::vector<int> counts;
    counts.reserve(h * w / 4);
    counts.resize(0);
    std::vector<int> avgs;
    avgs.reserve(h * w / 4);
    avgs.resize(0);
    int uid = 0;
    const int r = 1;
    int x_min, x_max, y_min, y_max;
    bool flag;

    // local vars
    cv::Point2i pt_;
    int ind;

    for (int i = 0; i < h; ++i) {
        for (int j = 0; j < w; ++j) {
            ind = i * w + j;
            if (data_edge[ind] || data_id[ind]) continue;
            // check if this point be a potential source
            x_min = std::max(0, i - r);
            x_max = std::min(h - 1, i + r);
            y_min = std::max(0, j - r);
            y_max = std::min(w - 1, j + r);
            flag = true;

            for (int s = x_min; s <= x_max && flag; ++s) {
                for (int t = y_min; t <= y_max; ++t) {
                    if (data_edge[s*w + t]) {
                        flag = false;
                        break;
                    }
                }
            }
            if (!flag) continue;
            // begin to search connected domains
            uid++;
            counts.push_back(0);
            avgs.push_back(0);
            // search for connected domain
            nodes.resize(0);
            nodes.push_back(cv::Point2i(i, j)); // center
            data_id[ind] = uid;
            counts[uid - 1]++;
            avgs[uid - 1] += data_i[ind];

            while (nodes.size() > 0) {
                pt_ = nodes.back();
                nodes.pop_back();
                // check if this point be a potential source
                x_min = std::max(0, pt_.x - r);
                x_max = std::min(h - 1, pt_.x + r);
                y_min = std::max(0, pt_.y - r);
                y_max = std::min(w - 1, pt_.y + r);
                flag = true;

                for (int s = x_min; s <= x_max && flag; ++s) {
                    for (int t = y_min; t <= y_max; ++t) {
                        if (data_edge[s*w + t]) {
                            flag = false;
                            break;
                        }
                    }
                }

                if (!flag) continue;

                // search for 4 neighbors
                if (pt_.x >= 1) {
                    ind = (pt_.x - 1)*w + pt_.y;
                    if (!data_edge[ind] && !data_id[ind]) {
                        nodes.push_back(cv::Point2i(pt_.x - 1, pt_.y)); // up
                        data_id[ind] = uid;
                        if (data_i[ind]) { // black holes do not count
                            counts[uid - 1]++;
                            avgs[uid - 1] += data_i[ind];
                        }
                    }
                }
                if (pt_.y >= 1) {
                    ind = pt_.x*w + pt_.y - 1;
                    if (!data_edge[ind] && !data_id[ind]) {
                        nodes.push_back(cv::Point2i(pt_.x, pt_.y - 1)); // left
                        data_id[ind] = uid;
                        if (data_i[ind]) { // black holes do not count
                            counts[uid - 1]++;
                            avgs[uid - 1] += data_i[ind];
                        }
                    }
                }
                if (pt_.y <= w - 2) {
                    ind = pt_.x*w + pt_.y + 1;
                    if (!data_edge[ind] && !data_id[ind]) {
                        nodes.push_back(cv::Point2i(pt_.x, pt_.y + 1)); // right
                        data_id[ind] = uid;
                        if (data_i[ind]) { // black holes do not count
                            counts[uid - 1]++;
                            avgs[uid - 1] += data_i[ind];
                        }
                    }
                }
                if (pt_.x <= h - 2) {
                    ind = (pt_.x + 1)*w + pt_.y;
                    if (!data_edge[ind] && !data_id[ind]) {
                        nodes.push_back(cv::Point2i(pt_.x + 1, pt_.y)); // bottom
                        data_id[ind] = uid;
                        if (data_i[ind]) { // black holes do not count
                            counts[uid - 1]++;
                            avgs[uid - 1] += data_i[ind];
                        }
                    }
                }
            }
        }
    }

    for (int i = 0; i < counts.size(); ++i) {
        if ( counts[i] >= min_support ) avgs[i] /= counts[i];
        else avgs[i] = 0;
    }

    // check out all connected domains
    int stride = 0;
    for (int i = 0; i < h; ++i) {
        for (int j = 0; j < w; ++j) {
            ind = stride + j;
            if (data_id[ind] && (!use_mask || !data_i[ind])) data_o[ind] = avgs[data_id[ind] - 1];
            else data_o[ind] = data_i[ind];
        }
        stride += w;
    }
}


void r3d_depth_image::SmoothDisparityWithDomains(
	cv::Mat & o_,
	const cv::Mat & i_,
	const cv::Mat & domains, // DOMAIN INDEX START FROM 1 INSTEAD OF 0
	int num_domains,
	const cv::Mat & mask
	) {
	if (i_.type() != CV_8UC1) {
		IMAGE_DEPTH_LOGE("[SmoothDisparityWithDomain] Assertion Failed: i_.type()!=CV_8UC1");
		WaitKey(0);
		exit(EXIT_FAILURE);		
	}
	if (domains.type() != CV_16SC1) {
		IMAGE_DEPTH_LOGE("[SmoothDisparityWithDomain] Assertion Failed: domains.type()!=CV_16SC1");
		WaitKey(0);
		exit(EXIT_FAILURE);
	}

	int h, w;
	h = i_.size().height;
	w = i_.size().width;
	if (o_.size() != i_.size() || o_.type() != i_.type()) {
		o_.create(i_.size(), i_.type());
	}

	unsigned char * pO = o_.data;
	const unsigned char * pI = i_.data;
	unsigned short * pD = (unsigned short *)(domains.data);
	const unsigned char * pM = mask.data;
	std::vector<float> max_disp;
	max_disp.resize(num_domains);

	// initialize the average disparity list
	for (int i = 0; i < max_disp.size(); ++i) max_disp[i] = 0;
	
	int pix_id, dom_id;
	for (int i = 0; i < h; ++i) {
		for (int j = 0; j < w; ++j) {
			pix_id = i*w + j;
			dom_id = pD[pix_id] - 1;
			if (dom_id >= 0 && pM[pix_id]) max_disp[dom_id] = std::fmax(max_disp[dom_id], pI[pix_id]);
		}
	}
	for (int i = 0; i < h; ++i) {
		for (int j = 0; j < w; ++j) {
			pix_id = i*w + j;
			dom_id = pD[pix_id] - 1;
			if (dom_id >= 0 && pM[pix_id]) pO[pix_id] = max_disp[dom_id];
			else pO[pix_id] = pI[pix_id];
		}
	}
}


void r3d_depth_image::GetFullEdges(cv::Mat & o_, const cv::Mat & i_,
                  int thres_selc, int thres_conn) {
    std::vector<cv::Mat> channels, edges;
    if (i_.type() == CV_8UC3) {
        cv::split(i_, channels);
        edges.resize(channels.size());
        cv::Canny(channels[0], edges[0], thres_conn, thres_selc);
        cv::Canny(channels[1], edges[1], thres_conn, thres_selc);
        cv::Canny(channels[2], edges[2], thres_conn, thres_selc);
        cv::bitwise_or(edges[0], edges[1], edges[0]);
        cv::bitwise_or(edges[0], edges[2], edges[0]);
        edges[0].copyTo(o_);
    }
    else if (i_.type() == CV_8UC1) {
        edges.resize(1);
        cv::Canny(i_, o_, thres_conn, thres_selc);
    }
    else {
		return;
    }
}


void r3d_depth_image::FindConnectedDomain(
        cv::Mat & domain_map,
        std::vector<int> & counts,
        cv::Rect & visible_zone,
        const cv::Mat & edge
) {
    if (edge.type() != CV_8UC1) {
		return;
    }
    if (domain_map.type() != CV_16SC1 && domain_map.size() != edge.size()) {
        domain_map.create(edge.size(), CV_16SC1);
    }
    counts.resize(0);

    int h, w;
    h = edge.size().height;
    w = edge.size().width;

    int bound_x[2], bound_y[2];

    bound_x[0] = visible_zone.x;
    bound_x[1] = visible_zone.x + visible_zone.width;
    bound_y[0] = visible_zone.y;
    bound_y[1] = visible_zone.y + visible_zone.height;

    const unsigned char * data_edge = (unsigned char *)(edge.data);
    unsigned char v;

    // create a label matrix to store labels for every pixel in main view
    short * data_id = (short *)(domain_map.data);
    std::vector<cv::Point2i> nodes;
    nodes.reserve(h * w / 4);
    nodes.resize(0);
    counts.reserve(h * w / 4);
    counts.resize(0);

    int uid = 0;
    const int r = 1;
    int x_min, x_max, y_min, y_max;
    bool flag;

    // local vars
    cv::Point2i pt_;
    int ind;

    for (int i = 0; i < h; ++i) {
        for (int j = 0; j < w; ++j) {
            data_id[i * w + j] = 0;
        }
    }

    for (int i = bound_y[0]; i < bound_y[1]; ++i) {
        for (int j = bound_x[0]; j < bound_x[1]; ++j) {
            ind = i * w + j;
            if (data_edge[ind] || data_id[ind]) continue;
            // check if this point be a potential source
            x_min = std::max(bound_x[0], j - r);
            x_max = std::min(bound_x[1] - 1, j + r);
            y_min = std::max(bound_y[0], i - r);
            y_max = std::min(bound_y[1] - 1, i + r);
            flag = true;

            for (int s = y_min; s <= y_max && flag; ++s) {
                for (int t = x_min; t <= x_max; ++t) {
                    if (data_edge[s*w + t]) {
                        flag = false;
                        break;
                    }
                }
            }
            if (!flag) continue;
            // begin to search connected domains
            uid++;
            counts.push_back(0);
            // search for connected domain
            nodes.resize(0);
            nodes.push_back(cv::Point2i(j, i)); // center
            data_id[ind] = uid;
            counts[uid - 1]++;

            while (nodes.size() > 0) {
                pt_ = nodes.back();
                nodes.pop_back();
                // check if this point be a potential source
                x_min = std::max(bound_x[0], pt_.x - r);
                x_max = std::min(bound_x[1] - 1, pt_.x + r);
                y_min = std::max(bound_y[0], pt_.y - r);
                y_max = std::min(bound_y[1] - 1, pt_.y + r);
                flag = true;

                for (int s = y_min; s <= y_max && flag; ++s) {
                    for (int t = x_min; t <= x_max; ++t) {
                        if (data_edge[s*w + t]) {
                            flag = false;
                            break;
                        }
                    }
                }

                if (!flag) continue;

                // search for 4 neighbors
                if (pt_.y >= bound_y[0] + 1) {
                    ind = (pt_.y - 1)*w + pt_.x;
                    if (!data_edge[ind] && !data_id[ind]) {
                        nodes.push_back(cv::Point2i(pt_.x, pt_.y - 1)); // up
                        data_id[ind] = uid;
                        counts[uid - 1]++;
                    }
                }
                if (pt_.x >= bound_x[0] + 1) {
                    ind = pt_.y*w + pt_.x - 1;
                    if (!data_edge[ind] && !data_id[ind]) {
                        nodes.push_back(cv::Point2i(pt_.x - 1, pt_.y)); // left
                        data_id[ind] = uid;
                        counts[uid - 1]++;
                    }
                }
                if (pt_.x <= bound_x[1] - 2) {
                    ind = pt_.y*w + pt_.x + 1;
                    if (!data_edge[ind] && !data_id[ind]) {
                        nodes.push_back(cv::Point2i(pt_.x + 1, pt_.y)); // right
                        data_id[ind] = uid;
                        counts[uid - 1]++;
                    }
                }
                if (pt_.y <= bound_y[1] - 2) {
                    ind = (pt_.y + 1)*w + pt_.x;
                    if (!data_edge[ind] && !data_id[ind]) {
                        nodes.push_back(cv::Point2i(pt_.x, pt_.y + 1)); // bottom
                        data_id[ind] = uid;
                        counts[uid - 1]++;
                    }
                }
            }
        }
    }

    // check if counter works well
    int num_pts = 0;
    for (int i = 0; i < counts.size(); ++i) {
        num_pts += counts[i];
    }
    if (num_pts > visible_zone.area()) {
		return;
    }
}


void r3d_depth_image::VisualizeDomains(cv::Mat & rgb, const cv::Mat & domains, int num_domain) {
    unsigned char color_table[7][3];
    // red
    color_table[0][0] = 0xff;
    color_table[0][1] = 0;
    color_table[0][2] = 0;
    // orange
    color_table[1][0] = 0xff;
    color_table[1][1] = 0x7f;
    color_table[1][2] = 0;
    // yellow
    color_table[2][0] = 0xff;
    color_table[2][1] = 0xff;
    color_table[2][2] = 0;
    // green
    color_table[3][0] = 0;
    color_table[3][1] = 0xff;
    color_table[3][2] = 0;
    // olive
    color_table[4][0] = 0;
    color_table[4][1] = 0xff;
    color_table[4][2] = 0xff;
    // blue
    color_table[5][0] = 0;
    color_table[5][1] = 0;
    color_table[5][2] = 0xff;
    // purple
    color_table[6][0] = 0x88;
    color_table[6][1] = 0;
    color_table[6][2] = 0xff;

    int h, w;
    h = domains.size().height;
    w = domains.size().width;
    int domain_id;

    rgb.create(domains.size(), CV_8UC3);
    unsigned char * p_rgb = (unsigned char *)(rgb.data);
    short * p_domain = (short *)(domains.data);
    int s;

    for (int i = 0; i < h; ++i) {
        s = i * w;
        for (int j = 0; j < w; ++j) {
            domain_id = p_domain[s + j];
            if (domain_id > 0) {
                p_rgb[3 * (s + j)] = color_table[domain_id % 7][0];
                p_rgb[3 * (s + j) + 1] = color_table[domain_id % 7][1];
                p_rgb[3 * (s + j) + 2] = color_table[domain_id % 7][2];
            }
            else {
                p_rgb[3 * (s + j)] = 0;
                p_rgb[3 * (s + j) + 1] = 0;
                p_rgb[3 * (s + j) + 2] = 0;
            }
        }
    }
}


void r3d_depth_image::ColorBasedFilter(
        cv::Mat & o_,
        const cv::Mat & i_,
        const cv::Mat & rgb,
        int r
) {
    if (i_.type() != CV_8UC1 ||
        rgb.type() != CV_8UC3 ||
        i_.size() != rgb.size()) {
		return;
    }
    if (o_.size() != i_.size() || o_.type() != i_.type()) {
        o_.create(i_.size(), i_.type());
    }

    unsigned char * p_o = o_.data;
    unsigned char * p_i = i_.data;
    const unsigned char * p_c = rgb.data;

    int h, w;
    h = i_.size().height;
    w = i_.size().width;
    unsigned char v[4]; // depth of neighbors
    unsigned char c[3]; // color of origin
    unsigned short d_u16[4]; // color distance in u16
    unsigned char d_u8[4]; // color distance in u8
    unsigned char d_mean_u8; // mean color distance in u8
    unsigned short sum_u16; // sum of weights in u16
    unsigned short v_u16; // the weighted sum of depth
    int addr_[4], addr_0;
    unsigned char zero_flag; // the flag for sum being zero or not

    // use non-uniform weighted sum to replace min/max
    for (int i = 0; i < h; ++i) {
        for (int j = 0; j < w; ++j) {
            // origin
            addr_0 = i * w + j;
            addr_0 *= 3;
            c[0] = p_c[addr_0];
            c[1] = p_c[addr_0 + 1];
            c[2] = p_c[addr_0 + 2];
            // neighbors
            zero_flag = i >= r;
            addr_[0] = zero_flag * (i - r) * w + j;
            zero_flag = i + r < h;
            addr_[1] = (zero_flag * (i + r) + (1 - zero_flag) * (h - 1)) * w + j;
            zero_flag = j >= r;
            addr_[2] = i * w + zero_flag * (j - r);
            zero_flag = j + r < w;
            addr_[3] = i * w + zero_flag * (j + r) + (1 - zero_flag) * (w - 1);
            v[0] = p_i[addr_[0]];
            v[1] = p_i[addr_[1]];
            v[2] = p_i[addr_[2]];
            v[3] = p_i[addr_[3]];
            // channel R
            addr_[0] *= 3;
            addr_[1] *= 3;
            addr_[2] *= 3;
            addr_[3] *= 3;
            d_u16[0] = std::abs(p_c[addr_[0]] - c[0]);
            d_u16[1] = std::abs(p_c[addr_[1]] - c[0]);
            d_u16[2] = std::abs(p_c[addr_[2]] - c[0]);
            d_u16[3] = std::abs(p_c[addr_[3]] - c[0]);
            // channel G
            addr_[0] ++;
            addr_[1] ++;
            addr_[2] ++;
            addr_[3] ++;
            d_u16[0] += std::abs(p_c[addr_[0]] - c[1]);
            d_u16[1] += std::abs(p_c[addr_[1]] - c[1]);
            d_u16[2] += std::abs(p_c[addr_[2]] - c[1]);
            d_u16[3] += std::abs(p_c[addr_[3]] - c[1]);
            // channel B
            addr_[0] ++;
            addr_[1] ++;
            addr_[2] ++;
            addr_[3] ++;
            d_u16[0] += std::abs(p_c[addr_[0]] - c[1]);
            d_u16[1] += std::abs(p_c[addr_[1]] - c[1]);
            d_u16[2] += std::abs(p_c[addr_[2]] - c[1]);
            d_u16[3] += std::abs(p_c[addr_[3]] - c[1]);
            // normalize color distance to uint8
            d_u8[0] = d_u16[0] >> 2;
            d_u8[1] = d_u16[1] >> 2;
            d_u8[2] = d_u16[2] >> 2;
            d_u8[3] = d_u16[3] >> 2;
            // get average of color distances
            sum_u16 = d_u8[0];
            sum_u16 += d_u8[1];
            sum_u16 += d_u8[2];
            sum_u16 += d_u8[3];
            d_mean_u8 = sum_u16 >> 2;
            // polarize color distances
            d_u8[0] = d_u8[0] <= d_mean_u8;
            d_u8[1] = d_u8[1] <= d_mean_u8;
            d_u8[2] = d_u8[2] <= d_mean_u8;
            d_u8[3] = d_u8[3] <= d_mean_u8;
            // get average of depth
            sum_u16 = v[0];
            sum_u16 += v[1];
            sum_u16 += v[2];
            sum_u16 += v[3];
            d_mean_u8 = sum_u16 >> 2;
            // sum of weights
            sum_u16 = d_u8[0];
            sum_u16 += d_u8[1];
            sum_u16 += d_u8[2];
            sum_u16 += d_u8[3];
            // get the final weighted depth
            v_u16 = v[0] * d_u8[0];
            v_u16 += v[1] * d_u8[1];
            v_u16 += v[2] * d_u8[2];
            v_u16 += v[3] * d_u8[3];
            v_u16 /= sum_u16;
            p_o[i * w + j] = v_u16;
        }
    }
}


class r3d_depth_image::Erode_v3_fastest_class : public cv::ParallelLoopBody
{
public:
    Erode_v3_fastest_class(cv::Mat & o,
                           const cv::Mat &i,
                           const cv::Mat &rgb_,
                           int r_,
                           int thread_num_,
                           uint8_t thr_
    ) {
        o_ = o;
        i_ = i;
        rgb = rgb_;
        r = r_;
        this->thr_ = thr_;

        h = i_.size().height;
        w = i_.size().width;

        i_step = i.step[0];
        rgb_step = rgb_.step[0];
        left_r = std::max(r, 8);

        r_x_w = r * i.step[0], r_x_3w = r * rgb_.step[0];
        r_x_3 = r * 3, w___r_x_3 = (w - left_r) * 3, h___1_x_3w = (h - 1) * rgb_.step[0];
        w___r = (w - left_r), h___1_x_w = (h - 1) * i.step[0];

        p_o_org = o_.data;
        p_i_org = i_.data;
        p_rgb_org = rgb.data;

        this->thread_num = thread_num_;
    }
    virtual void operator ()(const cv::Range& range) const
    {
        //for (int t = range.start; t < range.end; ++t)
        {
            //for(int i = t; i < h; i+=thread_num)
            for (int i = range.start; i < range.end; ++i)
            {
                size_t step_i = i_.step[0];
                size_t step_rgb = rgb.step[0];

                //SIMD gray center
                cv::v_int16x8 r_center_int16x8;
                cv::v_int16x8 r_top_int16x8;
                cv::v_int16x8 r_bottom_int16x8;
                cv::v_int16x8 r_left_int16x8;
                cv::v_int16x8 r_right_int16x8;
                cv::v_int16x8 r_mean_int16x8;

                cv::v_int16x8 g_center_int16x8;
                cv::v_int16x8 g_top_int16x8;
                cv::v_int16x8 g_bottom_int16x8;
                cv::v_int16x8 g_left_int16x8;
                cv::v_int16x8 g_right_int16x8;
                cv::v_int16x8 g_mean_int16x8;

                cv::v_int16x8 b_center_int16x8;
                cv::v_int16x8 b_top_int16x8;
                cv::v_int16x8 b_bottom_int16x8;
                cv::v_int16x8 b_left_int16x8;
                cv::v_int16x8 b_right_int16x8;
                cv::v_int16x8 b_mean_int16x8;

                cv::v_uint16x8 v_center_depth_uint16x8, v_top_depth_uint16x8,
                        v_bottom_depth_uint16x8, v_left_depth_uint16x8,
                        v_right_depth_uint16x8, v_mean_depth_uint16x8,
                        v_ones_uint16x8, v_zeros_uint16x8, v_weight_uint16x8, v_new_depth_uint16x8;

                cv::v_uint16x8 top_flag, bottom_flag, left_flag, right_flag, tmp_flag;

                cv::v_uint16x8 tmp_left_uint16x8, tmp_right_uint16x8;
                cv::v_uint8x16 tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16;

                v_ones_uint16x8 = cv::v_setall_u16(1);
                v_zeros_uint16x8 = cv::v_setall_u16(0);


                unsigned char * p_o_line = p_o_org + i * i_step;
                unsigned char * p_i_line = p_i_org + i * i_step;
                unsigned char * p_rgb_line = p_rgb_org + i * rgb_step;
                unsigned char * p_o = p_o_line;
                unsigned char * p_i = p_i_line;
                unsigned char * p_rgb = p_rgb_line;
                unsigned char * p_tmp;

                ushort v_new_depth_uint16_arr8[8], weight_depth_uint16_arr8[8];
                ushort *v_new_depth_uint16_arr8_pt0 = &(v_new_depth_uint16_arr8[0]);
                ushort *v_new_depth_uint16_arr8_pt1 = &(v_new_depth_uint16_arr8[1]);
                ushort *v_new_depth_uint16_arr8_pt2 = &(v_new_depth_uint16_arr8[2]);
                ushort *v_new_depth_uint16_arr8_pt3 = &(v_new_depth_uint16_arr8[3]);
                ushort *v_new_depth_uint16_arr8_pt4 = &(v_new_depth_uint16_arr8[4]);
                ushort *v_new_depth_uint16_arr8_pt5 = &(v_new_depth_uint16_arr8[5]);
                ushort *v_new_depth_uint16_arr8_pt6 = &(v_new_depth_uint16_arr8[6]);
                ushort *v_new_depth_uint16_arr8_pt7 = &(v_new_depth_uint16_arr8[7]);

                ushort *weight_depth_uint16_arr8_pt0 = &(weight_depth_uint16_arr8[0]);
                ushort *weight_depth_uint16_arr8_pt1 = &(weight_depth_uint16_arr8[1]);
                ushort *weight_depth_uint16_arr8_pt2 = &(weight_depth_uint16_arr8[2]);
                ushort *weight_depth_uint16_arr8_pt3 = &(weight_depth_uint16_arr8[3]);
                ushort *weight_depth_uint16_arr8_pt4 = &(weight_depth_uint16_arr8[4]);
                ushort *weight_depth_uint16_arr8_pt5 = &(weight_depth_uint16_arr8[5]);
                ushort *weight_depth_uint16_arr8_pt6 = &(weight_depth_uint16_arr8[6]);
                ushort *weight_depth_uint16_arr8_pt7 = &(weight_depth_uint16_arr8[7]);

                for (int j = 0; j < w; j += 8) {
                    ///////////r channel
                    cv::v_load_deinterleave(p_rgb, tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16);
                    cv::v_expand(tmp_r_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    r_center_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    cv::v_expand(tmp_g_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    g_center_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    cv::v_expand(tmp_b_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    b_center_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);

                    p_tmp = i >= r ? p_rgb - r_x_3w : p_rgb_org + (p_rgb - p_rgb_line);
                    cv::v_load_deinterleave(p_tmp, tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16);
                    cv::v_expand(tmp_r_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    r_top_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    cv::v_expand(tmp_g_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    g_top_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    cv::v_expand(tmp_b_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    b_top_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);

                    p_tmp = i < h - r ? p_rgb + r_x_3w : p_rgb_org + h___1_x_3w + (p_rgb - p_rgb_line);
                    cv::v_load_deinterleave(p_tmp, tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16);
                    cv::v_expand(tmp_r_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    r_bottom_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    cv::v_expand(tmp_g_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    g_bottom_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    cv::v_expand(tmp_b_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    b_bottom_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);

                    p_tmp = j >= r ? p_rgb - r_x_3 : p_rgb_line;
                    //p_tmp = p_rgb - r_x_3;
                    cv::v_load_deinterleave(p_tmp, tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16);
                    cv::v_expand(tmp_r_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    r_left_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    cv::v_expand(tmp_g_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    g_left_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    cv::v_expand(tmp_b_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    b_left_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);

                    p_tmp = j + left_r < w ? p_rgb + r_x_3 : p_rgb_line + w___r_x_3;
                    //p_tmp = p_rgb + r_x_3;
                    cv::v_load_deinterleave(p_tmp, tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16);
                    cv::v_expand(tmp_r_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    r_right_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    cv::v_expand(tmp_g_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    g_right_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    cv::v_expand(tmp_b_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                    b_right_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                    v_top_depth_uint16x8 = cv::v_absdiff(r_center_int16x8, r_top_int16x8) +
                                           cv::v_absdiff(g_center_int16x8, g_top_int16x8) +
                                           cv::v_absdiff(b_center_int16x8, b_top_int16x8);
                    v_bottom_depth_uint16x8 = cv::v_absdiff(r_center_int16x8, r_bottom_int16x8) +
                                              cv::v_absdiff(g_center_int16x8, g_bottom_int16x8) +
                                              cv::v_absdiff(b_center_int16x8, b_bottom_int16x8);
                    v_left_depth_uint16x8 = cv::v_absdiff(r_center_int16x8, r_left_int16x8) +
                                            cv::v_absdiff(g_center_int16x8, g_left_int16x8) +
                                            cv::v_absdiff(b_center_int16x8, b_left_int16x8);
                    v_right_depth_uint16x8 = cv::v_absdiff(r_center_int16x8, r_right_int16x8) +
                                             cv::v_absdiff(g_center_int16x8, g_right_int16x8) +
                                             cv::v_absdiff(b_center_int16x8, b_right_int16x8);

                    v_top_depth_uint16x8 = v_top_depth_uint16x8 >> 2;
                    v_bottom_depth_uint16x8 = v_bottom_depth_uint16x8 >> 2;
                    v_left_depth_uint16x8 = v_left_depth_uint16x8 >> 2;
                    v_right_depth_uint16x8 = v_right_depth_uint16x8 >> 2;

                    v_mean_depth_uint16x8 = (v_top_depth_uint16x8
                                             + v_bottom_depth_uint16x8
                                             + v_left_depth_uint16x8
                                             + v_right_depth_uint16x8) >> 2;

                    cv::v_uint16x8 v_min_color_diff = cv::v_setall_u16(thr_);
                    v_mean_depth_uint16x8 = cv::v_min(v_mean_depth_uint16x8, v_min_color_diff);

                    top_flag = v_top_depth_uint16x8 <= v_mean_depth_uint16x8;
                    bottom_flag = v_bottom_depth_uint16x8 <= v_mean_depth_uint16x8;
                    left_flag = v_left_depth_uint16x8 <= v_mean_depth_uint16x8;
                    right_flag = v_right_depth_uint16x8 <= v_mean_depth_uint16x8;

                    /////// depth map
                    //v_center_depth_uint16x8 = cv::v_load_expand(p_i);

                    p_tmp = i >= r ? p_i - r_x_w : p_i_org + j;
                    v_top_depth_uint16x8 = cv::v_load_expand(p_tmp);

                    p_tmp = i < h - r ? p_i + r_x_w : p_i_org + h___1_x_w + j;
                    v_bottom_depth_uint16x8 = cv::v_load_expand(p_tmp);

                    p_tmp = j >= r ? p_i - r : p_i_line;
                    //p_tmp = p_i - r;
                    v_left_depth_uint16x8 = cv::v_load_expand(p_tmp);

                    p_tmp = j + left_r < w ? p_i + r : p_i_line + w___r;
                    //p_tmp = p_i + r;
                    v_right_depth_uint16x8 = cv::v_load_expand(p_tmp);

                    v_mean_depth_uint16x8 = v_top_depth_uint16x8 + v_bottom_depth_uint16x8 + v_left_depth_uint16x8 + v_right_depth_uint16x8;
                    v_mean_depth_uint16x8 = v_mean_depth_uint16x8 >> 2;


                    top_flag = top_flag & (v_top_depth_uint16x8 <= v_mean_depth_uint16x8);
                    bottom_flag = bottom_flag & (v_bottom_depth_uint16x8 <= v_mean_depth_uint16x8);
                    left_flag = left_flag & (v_left_depth_uint16x8 <= v_mean_depth_uint16x8);
                    right_flag = right_flag & (v_right_depth_uint16x8 <= v_mean_depth_uint16x8);


                    v_new_depth_uint16x8 = cv::v_select(top_flag, v_top_depth_uint16x8, v_zeros_uint16x8);
                    v_new_depth_uint16x8 += cv::v_select(bottom_flag, v_bottom_depth_uint16x8, v_zeros_uint16x8);
                    v_new_depth_uint16x8 += cv::v_select(left_flag, v_left_depth_uint16x8, v_zeros_uint16x8);
                    v_new_depth_uint16x8 += cv::v_select(right_flag, v_right_depth_uint16x8, v_zeros_uint16x8);

                    v_weight_uint16x8 = cv::v_select(top_flag, v_ones_uint16x8, v_zeros_uint16x8);
                    v_weight_uint16x8 += cv::v_select(bottom_flag, v_ones_uint16x8, v_zeros_uint16x8);
                    v_weight_uint16x8 += cv::v_select(left_flag, v_ones_uint16x8, v_zeros_uint16x8);
                    v_weight_uint16x8 += cv::v_select(right_flag, v_ones_uint16x8, v_zeros_uint16x8);

                    //lol v_new_depth_uint16_arr8 no division op

                    cv::v_store(v_new_depth_uint16_arr8, v_new_depth_uint16x8);
                    cv::v_store(weight_depth_uint16_arr8, v_weight_uint16x8);
#ifndef MIN_SUPPORT
#define MIN_SUPPORT 1
#endif
                    *p_o = (*weight_depth_uint16_arr8_pt0 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt0) / (*weight_depth_uint16_arr8_pt0) : *p_i;
                    *(p_o + 1) = (*weight_depth_uint16_arr8_pt1 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt1) / (*weight_depth_uint16_arr8_pt1) : *(p_i + 1);
                    *(p_o + 2) = (*weight_depth_uint16_arr8_pt2 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt2) / (*weight_depth_uint16_arr8_pt2) : *(p_i + 2);
                    *(p_o + 3) = (*weight_depth_uint16_arr8_pt3 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt3) / (*weight_depth_uint16_arr8_pt3) : *(p_i + 3);
                    *(p_o + 4) = (*weight_depth_uint16_arr8_pt4 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt4) / (*weight_depth_uint16_arr8_pt4) : *(p_i + 4);
                    *(p_o + 5) = (*weight_depth_uint16_arr8_pt5 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt5) / (*weight_depth_uint16_arr8_pt5) : *(p_i + 5);
                    *(p_o + 6) = (*weight_depth_uint16_arr8_pt6 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt6) / (*weight_depth_uint16_arr8_pt6) : *(p_i + 6);
                    *(p_o + 7) = (*weight_depth_uint16_arr8_pt7 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt7) / (*weight_depth_uint16_arr8_pt7) : *(p_i + 7);

                    p_rgb += 24;
                    p_o += 8;
                    p_i += 8;
                }
            }
        }
    }

    Erode_v3_fastest_class& operator=(const Erode_v3_fastest_class &) {
        return *this;
    };

private:
    cv::Mat o_;
    cv::Mat i_;
    cv::Mat rgb;
    int r, thread_num;
    int h, w;
    size_t i_step, rgb_step;
    size_t r_x_w, r_x_3w;
    size_t r_x_3, w___r_x_3, h___1_x_3w;
    size_t w___r, h___1_x_w;
    size_t left_r;
    unsigned char * p_o_org;
    unsigned char * p_i_org;
    unsigned char * p_rgb_org;
    uint8_t thr_;
};


void r3d_depth_image::Erode_v3_fastest(
        cv::Mat & o_,
        const cv::Mat & i_,
        const cv::Mat & rgb,
        int r,
        int threads_num,
        uint8_t threshold
) {
    if (i_.type() != CV_8UC1 ||
        rgb.type() != CV_8UC3 ||
        i_.size() != rgb.size()) {
		return;
    }
    if (o_.size() != i_.size() || o_.type() != i_.type()) {
        o_.create(i_.size(), i_.type());
    }
    int org_threads_num = cv::getNumThreads();
    cv::setNumThreads(threads_num);
	r3d_depth_image::Erode_v3_fastest_class p_obj = 
		r3d_depth_image::Erode_v3_fastest_class(o_, i_, rgb, r, threads_num, threshold);
    cv::parallel_for_(cv::Range(0, i_.rows), p_obj);
    cv::setNumThreads(org_threads_num);
}



class r3d_depth_image::Dilate_v3_fastest_class : public cv::ParallelLoopBody
{
public:
    Dilate_v3_fastest_class(cv::Mat & o,
                            const cv::Mat &i,
                            const cv::Mat &rgb_,
                            int r_,
                            uint8_t thr_
    ) {
        o_ = o;
        i_ = i;
        rgb = rgb_;
        r = r_;
        this->thr_ = thr_;

        h = i_.size().height;
        w = i_.size().width;

        i_step = i.step[0];
        rgb_step = rgb_.step[0];

        left_r = std::max(r, 8);
        r_x_w = r * i.step[0], r_x_3w = r * rgb_.step[0];
        r_x_3 = r * 3, w___r_x_3 = (w - left_r) * 3, h___1_x_3w = (h - 1) * rgb_.step[0];
        w___r = (w - left_r), h___1_x_w = (h - 1) * i.step[0];

        p_o_org = o_.data;
        p_i_org = i_.data;
        p_rgb_org = rgb.data;
    }
    virtual void operator ()(const cv::Range& range) const
    {
        for (int i = range.start; i < range.end; ++i)
        {
            size_t step_i = i_.step[0];
            size_t step_rgb = rgb.step[0];

            //SIMD gray center
            cv::v_int16x8 r_center_int16x8;
            cv::v_int16x8 r_top_int16x8;
            cv::v_int16x8 r_bottom_int16x8;
            cv::v_int16x8 r_left_int16x8;
            cv::v_int16x8 r_right_int16x8;
            cv::v_int16x8 r_mean_int16x8;

            cv::v_int16x8 g_center_int16x8;
            cv::v_int16x8 g_top_int16x8;
            cv::v_int16x8 g_bottom_int16x8;
            cv::v_int16x8 g_left_int16x8;
            cv::v_int16x8 g_right_int16x8;
            cv::v_int16x8 g_mean_int16x8;

            cv::v_int16x8 b_center_int16x8;
            cv::v_int16x8 b_top_int16x8;
            cv::v_int16x8 b_bottom_int16x8;
            cv::v_int16x8 b_left_int16x8;
            cv::v_int16x8 b_right_int16x8;
            cv::v_int16x8 b_mean_int16x8;

            cv::v_uint16x8 v_center_depth_uint16x8, v_top_depth_uint16x8,
                    v_bottom_depth_uint16x8, v_left_depth_uint16x8,
                    v_right_depth_uint16x8, v_mean_depth_uint16x8,
                    v_ones_uint16x8, v_zeros_uint16x8, v_weight_uint16x8, v_new_depth_uint16x8;

            cv::v_uint16x8 top_flag, bottom_flag, left_flag, right_flag, tmp_flag;

            cv::v_uint16x8 tmp_left_uint16x8, tmp_right_uint16x8;
            cv::v_uint8x16 tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16;

            v_ones_uint16x8 = cv::v_setall_u16(1);
            v_zeros_uint16x8 = cv::v_setall_u16(0);


            unsigned char * p_o_line = p_o_org + i * i_step;
            unsigned char * p_i_line = p_i_org + i * i_step;
            unsigned char * p_rgb_line = p_rgb_org + i * rgb_step;
            unsigned char * p_o = p_o_line;
            unsigned char * p_i = p_i_line;
            unsigned char * p_rgb = p_rgb_line;
            unsigned char * p_tmp;

            ushort v_new_depth_uint16_arr8[8], weight_depth_uint16_arr8[8];
            ushort *v_new_depth_uint16_arr8_pt0 = &(v_new_depth_uint16_arr8[0]);
            ushort *v_new_depth_uint16_arr8_pt1 = &(v_new_depth_uint16_arr8[1]);
            ushort *v_new_depth_uint16_arr8_pt2 = &(v_new_depth_uint16_arr8[2]);
            ushort *v_new_depth_uint16_arr8_pt3 = &(v_new_depth_uint16_arr8[3]);
            ushort *v_new_depth_uint16_arr8_pt4 = &(v_new_depth_uint16_arr8[4]);
            ushort *v_new_depth_uint16_arr8_pt5 = &(v_new_depth_uint16_arr8[5]);
            ushort *v_new_depth_uint16_arr8_pt6 = &(v_new_depth_uint16_arr8[6]);
            ushort *v_new_depth_uint16_arr8_pt7 = &(v_new_depth_uint16_arr8[7]);

            ushort *weight_depth_uint16_arr8_pt0 = &(weight_depth_uint16_arr8[0]);
            ushort *weight_depth_uint16_arr8_pt1 = &(weight_depth_uint16_arr8[1]);
            ushort *weight_depth_uint16_arr8_pt2 = &(weight_depth_uint16_arr8[2]);
            ushort *weight_depth_uint16_arr8_pt3 = &(weight_depth_uint16_arr8[3]);
            ushort *weight_depth_uint16_arr8_pt4 = &(weight_depth_uint16_arr8[4]);
            ushort *weight_depth_uint16_arr8_pt5 = &(weight_depth_uint16_arr8[5]);
            ushort *weight_depth_uint16_arr8_pt6 = &(weight_depth_uint16_arr8[6]);
            ushort *weight_depth_uint16_arr8_pt7 = &(weight_depth_uint16_arr8[7]);

            for (int j = 0; j < w; j += 8) {
                ///////////r channel
                cv::v_load_deinterleave(p_rgb, tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16);
                cv::v_expand(tmp_r_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                r_center_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                cv::v_expand(tmp_g_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                g_center_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                cv::v_expand(tmp_b_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                b_center_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);

                p_tmp = i >= r ? p_rgb - r_x_3w : p_rgb_org + (p_rgb - p_rgb_line);
                cv::v_load_deinterleave(p_tmp, tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16);
                cv::v_expand(tmp_r_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                r_top_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                cv::v_expand(tmp_g_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                g_top_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                cv::v_expand(tmp_b_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                b_top_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);

                p_tmp = i < h - r ? p_rgb + r_x_3w : p_rgb_org + h___1_x_3w + (p_rgb - p_rgb_line);
                cv::v_load_deinterleave(p_tmp, tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16);
                cv::v_expand(tmp_r_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                r_bottom_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                cv::v_expand(tmp_g_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                g_bottom_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                cv::v_expand(tmp_b_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                b_bottom_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);

                p_tmp = j >= r ? p_rgb - r_x_3 : p_rgb_line;
                //p_tmp = p_rgb - r_x_3;
                cv::v_load_deinterleave(p_tmp, tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16);
                cv::v_expand(tmp_r_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                r_left_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                cv::v_expand(tmp_g_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                g_left_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                cv::v_expand(tmp_b_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                b_left_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);

                p_tmp = j + left_r < w ? p_rgb + r_x_3 : p_rgb_line + w___r_x_3;
                //p_tmp = p_rgb + r_x_3;
                cv::v_load_deinterleave(p_tmp, tmp_r_uint8x16, tmp_g_uint8x16, tmp_b_uint8x16);
                cv::v_expand(tmp_r_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                r_right_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                cv::v_expand(tmp_g_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                g_right_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                cv::v_expand(tmp_b_uint8x16, tmp_left_uint16x8, tmp_right_uint16x8);
                b_right_int16x8 = cv::v_reinterpret_as_s16(tmp_left_uint16x8);
                v_top_depth_uint16x8 = cv::v_absdiff(r_center_int16x8, r_top_int16x8) +
                                       cv::v_absdiff(g_center_int16x8, g_top_int16x8) +
                                       cv::v_absdiff(b_center_int16x8, b_top_int16x8);
                v_bottom_depth_uint16x8 = cv::v_absdiff(r_center_int16x8, r_bottom_int16x8) +
                                          cv::v_absdiff(g_center_int16x8, g_bottom_int16x8) +
                                          cv::v_absdiff(b_center_int16x8, b_bottom_int16x8);
                v_left_depth_uint16x8 = cv::v_absdiff(r_center_int16x8, r_left_int16x8) +
                                        cv::v_absdiff(g_center_int16x8, g_left_int16x8) +
                                        cv::v_absdiff(b_center_int16x8, b_left_int16x8);
                v_right_depth_uint16x8 = cv::v_absdiff(r_center_int16x8, r_right_int16x8) +
                                         cv::v_absdiff(g_center_int16x8, g_right_int16x8) +
                                         cv::v_absdiff(b_center_int16x8, b_right_int16x8);

                v_top_depth_uint16x8 = v_top_depth_uint16x8 >> 2;
                v_bottom_depth_uint16x8 = v_bottom_depth_uint16x8 >> 2;
                v_left_depth_uint16x8 = v_left_depth_uint16x8 >> 2;
                v_right_depth_uint16x8 = v_right_depth_uint16x8 >> 2;

                v_mean_depth_uint16x8 = (v_top_depth_uint16x8
                                         + v_bottom_depth_uint16x8
                                         + v_left_depth_uint16x8
                                         + v_right_depth_uint16x8) >> 2;

                cv::v_uint16x8 v_min_color_diff = cv::v_setall_u16(thr_);
                v_mean_depth_uint16x8 = cv::v_min(v_mean_depth_uint16x8, v_min_color_diff);

                top_flag = v_top_depth_uint16x8 <= v_mean_depth_uint16x8;
                bottom_flag = v_bottom_depth_uint16x8 <= v_mean_depth_uint16x8;
                left_flag = v_left_depth_uint16x8 <= v_mean_depth_uint16x8;
                right_flag = v_right_depth_uint16x8 <= v_mean_depth_uint16x8;



                /////// depth map
                //v_center_depth_uint16x8 = cv::v_load_expand(p_i);

                p_tmp = i >= r ? p_i - r_x_w : p_i_org + j;
                v_top_depth_uint16x8 = cv::v_load_expand(p_tmp);

                p_tmp = i < h - r ? p_i + r_x_w : p_i_org + h___1_x_w + j;
                v_bottom_depth_uint16x8 = cv::v_load_expand(p_tmp);

                p_tmp = j >= r ? p_i - r : p_i_line;
                //p_tmp = p_i - r;
                v_left_depth_uint16x8 = cv::v_load_expand(p_tmp);

                p_tmp = j + left_r < w ? p_i + r : p_i_line + w___r;
                //p_tmp = p_i + r;
                v_right_depth_uint16x8 = cv::v_load_expand(p_tmp);

                v_mean_depth_uint16x8 = v_top_depth_uint16x8 + v_bottom_depth_uint16x8 + v_left_depth_uint16x8 + v_right_depth_uint16x8;
                v_mean_depth_uint16x8 = v_mean_depth_uint16x8 >> 2;


                top_flag = top_flag & (v_top_depth_uint16x8 >= v_mean_depth_uint16x8);
                bottom_flag = bottom_flag & (v_bottom_depth_uint16x8 >= v_mean_depth_uint16x8);
                left_flag = left_flag & (v_left_depth_uint16x8 >= v_mean_depth_uint16x8);
                right_flag = right_flag & (v_right_depth_uint16x8 >= v_mean_depth_uint16x8);


                v_new_depth_uint16x8 = cv::v_select(top_flag, v_top_depth_uint16x8, v_zeros_uint16x8);
                v_new_depth_uint16x8 += cv::v_select(bottom_flag, v_bottom_depth_uint16x8, v_zeros_uint16x8);
                v_new_depth_uint16x8 += cv::v_select(left_flag, v_left_depth_uint16x8, v_zeros_uint16x8);
                v_new_depth_uint16x8 += cv::v_select(right_flag, v_right_depth_uint16x8, v_zeros_uint16x8);

                v_weight_uint16x8 = cv::v_select(top_flag, v_ones_uint16x8, v_zeros_uint16x8);
                v_weight_uint16x8 += cv::v_select(bottom_flag, v_ones_uint16x8, v_zeros_uint16x8);
                v_weight_uint16x8 += cv::v_select(left_flag, v_ones_uint16x8, v_zeros_uint16x8);
                v_weight_uint16x8 += cv::v_select(right_flag, v_ones_uint16x8, v_zeros_uint16x8);

                //T_T v_new_depth_uint16_arr8 no division

                cv::v_store(v_new_depth_uint16_arr8, v_new_depth_uint16x8);
                cv::v_store(weight_depth_uint16_arr8, v_weight_uint16x8);
#ifndef MIN_SUPPORT
#define MIN_SUPPORT 1
#endif
                *p_o = (*weight_depth_uint16_arr8_pt0 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt0) / (*weight_depth_uint16_arr8_pt0) : *p_i;
                *(p_o + 1) = (*weight_depth_uint16_arr8_pt1 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt1) / (*weight_depth_uint16_arr8_pt1) : *(p_i + 1);
                *(p_o + 2) = (*weight_depth_uint16_arr8_pt2 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt2) / (*weight_depth_uint16_arr8_pt2) : *(p_i + 2);
                *(p_o + 3) = (*weight_depth_uint16_arr8_pt3 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt3) / (*weight_depth_uint16_arr8_pt3) : *(p_i + 3);
                *(p_o + 4) = (*weight_depth_uint16_arr8_pt4 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt4) / (*weight_depth_uint16_arr8_pt4) : *(p_i + 4);
                *(p_o + 5) = (*weight_depth_uint16_arr8_pt5 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt5) / (*weight_depth_uint16_arr8_pt5) : *(p_i + 5);
                *(p_o + 6) = (*weight_depth_uint16_arr8_pt6 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt6) / (*weight_depth_uint16_arr8_pt6) : *(p_i + 6);
                *(p_o + 7) = (*weight_depth_uint16_arr8_pt7 >= MIN_SUPPORT) ? (*v_new_depth_uint16_arr8_pt7) / (*weight_depth_uint16_arr8_pt7) : *(p_i + 7);

                p_rgb += 24;
                p_o += 8;
                p_i += 8;
            }
        }
    }

    Dilate_v3_fastest_class& operator=(const Dilate_v3_fastest_class &) {
        return *this;
    };

private:
    cv::Mat o_;
    cv::Mat i_;
    cv::Mat rgb;
    int r;
    int h, w;
    size_t i_step, rgb_step;
    size_t r_x_w, r_x_3w;
    size_t r_x_3, w___r_x_3, h___1_x_3w;
    size_t w___r, h___1_x_w;
    size_t left_r;
    unsigned char * p_o_org;
    unsigned char * p_i_org;
    unsigned char * p_rgb_org;
    uint8_t thr_;
};


void r3d_depth_image::Dilate_v3_fastest(
        cv::Mat & o_,
        const cv::Mat & i_,
        const cv::Mat & rgb,
        int r,
        int threads_num,
        uint8_t threshold
) {
    if (i_.type() != CV_8UC1 ||
        rgb.type() != CV_8UC3 ||
        i_.size() != rgb.size()) {
		return;
    }
    if (o_.size() != i_.size() || o_.type() != i_.type()) {
        o_.create(i_.size(), i_.type());
    }
    int org_threads_num = cv::getNumThreads();
    cv::setNumThreads(threads_num);
	r3d_depth_image::Dilate_v3_fastest_class p_obj = 
		r3d_depth_image::Dilate_v3_fastest_class(o_, i_, rgb, r, threshold);
    cv::parallel_for_(cv::Range(0, i_.rows), p_obj);
    cv::setNumThreads(org_threads_num);
}


void r3d_depth_image::FillBlackBorder(cv::Mat & disp_pri, int std_max_disp, int win_size) {

	int w = disp_pri.size().width;
	int h = disp_pri.size().height;

	if (disp_pri.type() == CV_8UC1)
	{
		
		unsigned char * data = disp_pri.data;
		int blank_width = std_max_disp + win_size / 2;

		int idx = blank_width;
		int stride = 0;

		for (int i = 0; i < h; ++i) {
			for (int j = 0; j < blank_width; ++j) {
				data[stride + j] = data[idx];
			}
			stride += w;
			idx += w;
		}
	}
	else if (disp_pri.type() == CV_16SC1)
	{
		for (int m = 0; m < h; m++)
		{
			short * ptr_ = disp_pri.ptr<short>(m);
			for (int n = 36; n > 1; n--)
			{
				ptr_[n - 1] = ptr_[n];
			}
		}
	}
}


void r3d_depth_image::Histogram(cv::Mat & hist, const cv::Mat & depth, int hist_size){
    float range[] = {0, 256};
    const float * histRanges = { range };
    cv::calcHist(&depth, 1, 0, cv::Mat(), hist, 1,
                 &hist_size, &histRanges, true, false);
    hist = hist / (depth.size().width * depth.size().height);
}


void r3d_depth_image::VisualizeHistogram(cv::String title, cv::Mat & hist){
    // visualize the histogram
    int hist_h = 400;
    int hist_w = 512;
    int hist_size = hist.size().height;
    int bin_w = hist_w / hist_size;
    cv::Mat hist_image(hist_w, hist_h, CV_8UC3, cv::Scalar(0, 0, 0));
    cv::Mat hist_norm;
    cv::normalize(hist, hist_norm, 0, hist_h,
                  cv::NORM_MINMAX, -1, cv::Mat());

    for (int i = 1; i < hist_size; i++) {
        cv::line(
                hist_image,
                cv::Point((i - 1)*bin_w,
                          hist_h - round(hist_norm.at<float>(i - 1))),
                cv::Point(i*bin_w,
                          hist_h - round(hist_norm.at<float>(i))),
                cv::Scalar(255, 0, 0), 2, cv::LINE_AA);
    }
}


void r3d_depth_image::StaticStereoRectify(
        const cv::Mat & M1,
        const cv::Mat & D1,
        const cv::Mat & M2,
        const cv::Mat & D2,
        const cv::Mat & R,
        const cv::Mat & T,
        const cv::Size & size,
        cv::Mat & map11,
        cv::Mat & map12,
        cv::Mat & map21,
        cv::Mat & map22,
        cv::Mat & imap1,
        cv::Mat & imap2,
        cv::Mat & Q
){
    // code to put here comes from Calibration Team,
    // required to meet Func API as follow:
    // StaticStereoRectify(M1, D1, M2, D2, R, T, size, map11, map12, map21, map22, imap1, imap2, Q);
    // {M1, D1, M2, D2 , R, T, size} are inputs;
    // map11-map22 are output forward maps;
    // imap1-imap2 are output backward maps for main camera only.
    // Q is the matrix to transform disparity into physical distance.

    // normalize both views
    cv::Mat R1, P1, R2, P2;
    cv::stereoRectify(M1, D1, M2, D2, size, R, T,
                      R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, 1, size);
    cv::initUndistortRectifyMap(M1, D1, R1, P1, size, CV_32F, map11, map12);
    cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_32F, map21, map22);

    // inverse map for the right view only
    imap1 = Mat::zeros(size, CV_32FC1);
    imap2 = Mat::zeros(size, CV_32FC1);

    std::vector<cv::Point2f> pts, pts_dst;

    pts.resize(size.width * size.height);
    pts_dst.resize(size.width * size.height);

    for (int i = 0; i < size.height; i++) {
        for (int j = 0; j < size.width; j++) {
            pts[i * size.width + j].x = j;
            pts[i * size.width + j].y = i;
        }
    }

#pragma omp parallel for num_threads(4)
    for (int idx = 0; idx < 4; idx++) {
        std::vector<cv::Point2f> pts_temp(
                pts.begin() + idx * size.width * size.height / 4,
                pts.begin() + (idx + 1) * size.width * size.height / 4);
        std::vector<cv::Point2f> pts_temp2(
                pts_dst.begin() + idx * size.width * size.height / 4,
                pts_dst.begin() + (idx + 1) * size.width * size.height / 4);
        cv::undistortPoints(
                pts_temp, pts_temp2, M2, D2, R2, P2);
        std::copy(
                pts_temp2.begin(),
                pts_temp2.end(),
                pts_dst.begin() +
                idx * size.width * size.height / 4);
    }

    for (int i = 0; i < size.height; i++) {
        for (int j = 0; j < size.width; j++) {
            imap1.at<float>(i, j) = pts_dst[i * size.width + j].x;
            imap2.at<float>(i, j) = pts_dst[i * size.width + j].y;
        }
    }
    pts.erase(pts.begin(), pts.end());
    pts_dst.erase(pts_dst.begin(), pts_dst.end());
}


void r3d_depth_image::GetRightPoint(std::vector<cv::Point2f> &rightVec,
	std::vector<cv::Point2f> &leftVec,
	std::vector<cv::Point2f> &rightVecOut,
	std::vector<cv::Point2f> &leftVecOut,
	std::vector<uchar> &status,
	float ratio
	){
	const float GRP_POINT_SCORE = 0.15;//0.2
	const float GRP_POINT_INTERVAL = 0.01;
	const int maxnum = 4000;

	int i, j;
	double dx, dy, dx1, dy1, dx2, dy2;
	float nx[maxnum];
	float ny[maxnum];
	float sumi[maxnum];
	float norm[maxnum];

	int numpoint;
	int distflag;
	float th;
	float n2;

	numpoint = rightVec.size();

	if (numpoint > maxnum) {
		IMAGE_DEPTH_LOGE("ASSERTION ERROR: [GetRightPoint]");
		exit(-1);
	}

	//if (numpoint>0)
	{
		for (i = 0; i<numpoint; i++)
		{
			/*if (status[i])
			{*/
			dx = rightVec[i].x - leftVec[i].x;
			dy = rightVec[i].y - leftVec[i].y;

			norm[i] = sqrt(dx*dx + dy*dy);

			if (norm[i] > 1e-6)
			{
				dx = dx / norm[i];
				dy = dy / norm[i];
			}
			else
			{
				dx = 0;
				dy = 0;
			}
			nx[i] = dx;//std v vector
			ny[i] = dy;//std v vector
					   //}
					   //else
					   //{
					   //	nx[i] = 0;//std v vector
					   //	ny[i] = 0;//std v vector
					   //}

		}
		//compute score
		for (i = 0; i<numpoint; i++)
		{

			if (!status[i])
			{
				sumi[i] = 0;       //score = 0 when status = 0
			}
			else
			{
				sumi[i] = 0;
				dx1 = rightVec[i].x - leftVec[i].x;//
				dy1 = rightVec[i].y - leftVec[i].y;
				for (j = 0; j<numpoint; j++)
				{
					distflag = 1;
					dx2 = rightVec[j].x - leftVec[j].x;//
					dy2 = rightVec[j].y - leftVec[j].y;
					distflag = ((dx1 - dx2)*(dx1 - dx2) + (dy1 - dy2)*(dy1 - dy2))>5.0 ? 0 : 1;//distance too large, the score is 0;


					if ((norm[i]<GRP_POINT_INTERVAL) && (norm[j]<GRP_POINT_INTERVAL))
						n2 = 1;
					else
						n2 = nx[i] * nx[j] + ny[i] * ny[j];

					sumi[i] = sumi[i] + n2*status[j] * distflag;//compute score, score=0 when distance too large or status=0 
				}

			}
			//posi[i] = i;//posi record init sequence

		}

		float max = 0.0;

		for (i = 0; i < numpoint; i++)
		{
			if (sumi[i] >= max)
			{
				max = sumi[i];
			}
		}

		//th = GRP_POINT_SCORE * max;
		th = ratio * max;

		//only score >= th is valid
		for (i = 0; i<numpoint; i++)
		{
			if (sumi[i] >= th)
			{
				rightVecOut.push_back(rightVec[i]);
				leftVecOut.push_back(leftVec[i]);
			}
		}


	}

}


 //find a transform matrix to align the two views in rows,
 //and strech the image to cover the maximum of the view
int r3d_depth_image::RowAlign_grid(
        cv::Mat & im2_aligned, // rectified aux
        const cv::Mat & im1, // main
        const cv::Mat & im2, // aux
        int num_feat,
        int max_iter,
        float res_level,
        int num_worst,
		int margin_x,
		int margin_y,
		float & max_disp,
		float & min_disp,
		cv::Mat & row_align
) {
	IMAGE_DEPTH_LOGI("\t RowAlign_grid BEGIN");

	double tick_freq = cv::getTickFrequency();
	double tick_start = TICK(), tick_stop;
	double tick_sub_start = tick_start, tick_sub_stop;

	cv::Mat M;
	float fx, fy, ppx, ppy;

	ppx = im1.size().width / 2.0;
	ppy = im1.size().height / 2.0;

	fx = 1;
	fy = 1;

	M.create(3, 3, CV_32F);
	M.at<float>(0, 0) = fx;
	M.at<float>(0, 1) = 0;
	M.at<float>(0, 2) = ppx;
	M.at<float>(1, 0) = 0;
	M.at<float>(1, 1) = fy;
	M.at<float>(1, 2) = ppy;
	M.at<float>(2, 0) = 0;
	M.at<float>(2, 1) = 0;
	M.at<float>(2, 2) = 1;
	
	cv::Mat tracker_im1, tracker_im2;

	cv::Mat mask;
	int total;
	int num_good;
	std::vector<cv::Point2f> pts1_good, pts2_good;
	cv::Mat map11, map12, map21, map22;
	int i, iter_;
	int ransac_used;
	float min_[2], max_[2], miu_[3], sigma_[3];
	float sum_[3], sum_of_square[3];
	int nSigma = 3;
	
	cv::cvtColor(im1, im2_aligned, cv::COLOR_RGB2GRAY);

	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t rgb2gray: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	cv::Mat left_gray, right_gray;
	cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
	clahe->apply(im2, left_gray);
	clahe->apply(im2_aligned, right_gray);

	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t clahe: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;
	
	std::vector<cv::Point2f> left_pts;
	std::vector<cv::Point2f> right_pts;
	
	int w_ = im1.cols, h_ = im1.rows;
	int nrow = 0, ncol = 0;
	int block_w = 15;
	int block_h = 15;
	for (int y = margin_y; y < h_-margin_y; y += block_h){
		nrow++;
		ncol = 0;
		for (int x = margin_x; x < w_-margin_x; x += block_w){
			ncol++;
			right_pts.push_back(cv::Point2f(x, y));
		}
	}

	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t create grid points: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	std::vector<uchar> status;
	std::vector<float> err;

	cv::calcOpticalFlowPyrLK(right_gray, left_gray, right_pts, left_pts, status, err, cv::Size(21, 21), 3);

	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t calc optical flow: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

#if SHOW_WINDOWS == 1
	cv::Mat tmp_out;
	im1.copyTo(tmp_out);
	VisualizeOpticalFlow(tmp_out, right_pts, left_pts);
	ShowImg("plk", tmp_out);
#endif

	IMAGE_DEPTH_LOGI("\t\t round 1 points: %lu", right_pts.size());
	if (right_pts.size() < 10) {
		im2.copyTo(im2_aligned);
		tick_stop = TICK();
		IMAGE_DEPTH_LOGI("\t RowAlign_grid DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));
		return 0;
	}
	
	std::vector<cv::Point2f> leftPoints, rightPoints;
	GetRightPoint(right_pts, left_pts, rightPoints, leftPoints, status, 0.2);
	
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t GetRightPoint: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	IMAGE_DEPTH_LOGI("\t\t round 2 points: %lu", rightPoints.size());
	if (rightPoints.size() < 10) {
		im2.copyTo(im2_aligned);
		tick_stop = TICK();
		IMAGE_DEPTH_LOGI("\t RowAlign_grid DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));
		return 0;
	}

#if SHOW_WINDOWS == 1
	cv::Mat tmp_out1;
	im1.copyTo(tmp_out1);
	VisualizeOpticalFlow(tmp_out1, rightPoints, leftPoints);
	ShowImg("plk_select", tmp_out1);
#endif

	// 3-sigma filter
	pts1_good.resize(rightPoints.size());
	pts2_good.resize(leftPoints.size());

	min_[0] = min_[1] = 1e5;
	max_[0] = max_[1] = -1e5;

	sum_[0] = sum_[1] = sum_[2] = 0;
	sum_of_square[0] = sum_of_square[1] = sum_of_square[2] = 0;

	float dx, dy;
	for (i = 0; i < rightPoints.size(); ++i) {
		dx = leftPoints[i].x - rightPoints[i].x;
		dy = leftPoints[i].y - rightPoints[i].y;
		sum_[0] += dx;
		sum_[1] += dy;
		sum_[2] += atan(dy / (dx + 1));

		if (dx < min_[0]) min_[0] = dx;
		if (dx > max_[0]) max_[0] = dx;
		if (dy < min_[1]) min_[1] = dy;
		if (dy > max_[1]) max_[1] = dy;
	}

	miu_[0] = sum_[0] / rightPoints.size();
	miu_[1] = sum_[1] / rightPoints.size();
	miu_[2] = sum_[2] / rightPoints.size();

	for (i = 0; i < rightPoints.size(); ++i) {
		dx = leftPoints[i].x - rightPoints[i].x;
		dy = leftPoints[i].y - rightPoints[i].y;
		sum_of_square[0] += (dx - miu_[0]) * (dx - miu_[0]);
		sum_of_square[1] += (dy - miu_[1]) * (dy - miu_[1]);
		sum_of_square[2] += (atan(dy / (dx + 1)) - miu_[2]) * (atan(dy / (dx + 1)) - miu_[2]);
	}

	sigma_[0] = sqrt(sum_of_square[0] / (rightPoints.size() - 1));
	sigma_[1] = sqrt(sum_of_square[1] / (rightPoints.size() - 1));
	sigma_[2] = sqrt(sum_of_square[2] / (rightPoints.size() - 1));

	pts1_good.resize(rightPoints.size());
	pts2_good.resize(leftPoints.size());

	for (i = 0, num_good = 0; i < rightPoints.size(); ++i) {
		dx = leftPoints[i].x - rightPoints[i].x;
		dy = leftPoints[i].y - rightPoints[i].y;
		if (abs(dx - miu_[0]) < nSigma * sigma_[0] &&
			abs(dy - miu_[1]) < nSigma * sigma_[1] &&
			abs(atan(dy / (dx + 1)) - miu_[2]) < nSigma * sigma_[2]) {
			pts1_good[num_good] = rightPoints[i];
			pts2_good[num_good] = leftPoints[i];
			++num_good;
		}
	}

	pts1_good.resize(num_good);
	pts2_good.resize(num_good);

	std::swap(rightPoints, pts1_good);
	std::swap(leftPoints, pts2_good);
	
	IMAGE_DEPTH_LOGI("\t\t round 3 points: %lu", rightPoints.size());
	if (rightPoints.size() < 10) {
		im2.copyTo(im2_aligned);
		tick_stop = TICK();
		IMAGE_DEPTH_LOGI("\t RowAlign_grid DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));
		return 0;
	}

#if SHOW_WINDOWS == 1
	cv::Mat tmp_out2;
	im1.copyTo(tmp_out2);
	VisualizeOpticalFlow(tmp_out2, rightPoints, leftPoints);
	ShowImg("plk_3sigma", tmp_out2);
#endif

	// solve row-aligning transform matrix with dynamic training samples
	int j;
	float u, v, u1, v1;
	float a21, a22, a23, a31, a32;
	int cntr;
	//num_worst=3
	float max_res[3], avg_res, this_res;
	std::vector<unsigned char> mask_accepted;
	int worst_pairs[3], total_pairs;
	Mat mat_x, vec_y, vec_beta, vec_betaT, mat_xT, mat_xTx, mat_xTx_inv, vec_res;

	mat_x.create(rightPoints.size(), 5, CV_32F);
	vec_y.create(rightPoints.size(), 1, CV_32F);
	vec_beta.create(5, 1, CV_32F);

	float * ptr_x = (float*)mat_x.data;
	float * ptr_y = (float*)vec_y.data;

	mask_accepted.resize(rightPoints.size());
	ransac_used = rightPoints.size();
	avg_res = 1e5;

	if (ransac_used < num_worst + 10) {
		im2.copyTo(im2_aligned);
		return 0; // failed with auto alignment
	}

	while (avg_res > res_level) {
		mat_x.resize(ransac_used);
		vec_y.resize(ransac_used);
		for (int i = 0; i < ransac_used; ++i)
		{
			v1 = (rightPoints[i].y - ppy) / fy;
			u = (leftPoints[i].x - ppx) / fx;
			v = (leftPoints[i].y - ppy) / fy;
			ptr_x[i * 5] = u;
			ptr_x[i * 5 + 1] = v;
			ptr_x[i * 5 + 2] = 1;
			ptr_x[i * 5 + 3] = -u * v1;
			ptr_x[i * 5 + 4] = -v * v1;
			ptr_y[i] = v1;
		}
		cv::transpose(mat_x, mat_xT);
		mat_xTx = mat_xT * mat_x;
		if (cv::invert(mat_xTx, mat_xTx_inv) == 0) {
			// this approach failed with bad selecting point pairs
			im2.copyTo(im2_aligned);
			return 0; // failed with auto alignment
		}
		vec_beta = mat_xT * vec_y;
		vec_beta = mat_xTx_inv * vec_beta;
		// apply the solved model to all samples to find out the worst N cases
		vec_res = mat_x * vec_beta;
		vec_res = cv::abs(vec_res - vec_y);
		avg_res = 0;
		max_res[0] = 0;
		cntr = 0;
		for (int i = 0; i < ransac_used; ++i) {
			this_res = vec_res.at<float>(i);
			avg_res += this_res;
			if (this_res >= max_res[0]) {
				for (j = num_worst - 1; j > 0; --j) {
					max_res[j] = max_res[j - 1];
					worst_pairs[j] = worst_pairs[j - 1];
				}
				max_res[0] = this_res;
				worst_pairs[0] = i;
				++cntr;
			}
		}
		// update average residual error for this round
		avg_res = avg_res / ransac_used;
		// filter out these worst N cases
		for (int i = 0; i < ransac_used; ++i) {
			mask_accepted[i] = 1;
		}
		cntr = cntr < num_worst ? cntr : num_worst;
		for (int i = 0; i < cntr; ++i) {
			mask_accepted[worst_pairs[i]] = 0;
		}
		total_pairs = ransac_used;
		ransac_used = 0;
		for (int i = 0; i < total_pairs; ++i) {
			if (mask_accepted[i]) {
				pts1_good[ransac_used] = rightPoints[i];
				pts2_good[ransac_used] = leftPoints[i];
				++ransac_used;
			}
		}
		pts1_good.resize(ransac_used);
		pts2_good.resize(ransac_used);
		std::swap(rightPoints, pts1_good);
		std::swap(leftPoints, pts2_good);
			
		// check if the points are enough
		if (ransac_used < 10) {
			im2.copyTo(im2_aligned);
			return 0; // faild with no enough points
		}
	}
	
	IMAGE_DEPTH_LOGI("\t\t round 4 points: %lu", rightPoints.size());
	if (rightPoints.size() < 10) {
		im2.copyTo(im2_aligned);
		tick_stop = TICK();
		IMAGE_DEPTH_LOGI("\t RowAlign_grid DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));
		return 0;
	}

#if SHOW_WINDOWS == 1
	cv::Mat tmp_out3;
	im1.copyTo(tmp_out3);
	VisualizeOpticalFlow(tmp_out3, rightPoints, leftPoints);
	ShowImg("plk_ransac", tmp_out3);
#endif

	a21 = vec_beta.at<float>(0);
	a22 = vec_beta.at<float>(1);
	a23 = vec_beta.at<float>(2);
	a31 = vec_beta.at<float>(3);
	a32 = vec_beta.at<float>(4);

	cv::Mat mat_row_align;
	mat_row_align.create(3, 3, CV_32F);
	mat_row_align.at<float>(0, 0) = 1;
	mat_row_align.at<float>(0, 1) = 0;
	mat_row_align.at<float>(0, 2) = 0;
	mat_row_align.at<float>(1, 0) = a21;
	mat_row_align.at<float>(1, 1) = a22;
	mat_row_align.at<float>(1, 2) = a23;
	mat_row_align.at<float>(2, 0) = a31;
	mat_row_align.at<float>(2, 1) = a32;
	mat_row_align.at<float>(2, 2) = 1;

	// Use Shear Transformation to restore the image in good shape
	// find the center points on 4 borders transformed using row-aligning matrix
	cv::Point2f centers[4];
	float a11, a12, a13;
	float h, w;
	h = im2.size().height;
	w = im2.size().width;
	centers[0].x = (w - 1) / 2.0;
	centers[0].y = 0;
	centers[1].x = w - 1;
	centers[1].y = (h - 1) / 2.0;
	centers[2].x = (w - 1) / 2.0;
	centers[2].y = h - 1;
	centers[3].x = 0;
	centers[3].y = (h - 1) / 2.0;

	// apply the row-align transform
	for (int i = 0; i < 4; ++i) {
		u = (centers[i].x - ppx) / fx;
		v = (centers[i].y - ppy) / fy;
		centers[i].x = (u / (a31 * u + a32 * v + 1)) * fx + ppx;
		centers[i].y = ((a21 * u + a22 * v + a23) / (a31 * u + a32 * v + 1)) * fy + ppy;
	}

	cv::Point2f u_, v_;
	u_ = centers[1] - centers[3];
	v_ = centers[0] - centers[2];

	a11 = (h * h * u_.y * u_.y + w * w * v_.y * v_.y) / (h * w * (u_.y * v_.x - u_.x * v_.y));
	a12 = (h * h * u_.x * u_.y + w * w * v_.x * v_.y) / (h * w * (u_.x * v_.y - u_.y * v_.x));

	float du_mean = 0;
	float du_sigma = 0;
	std::vector<int> disp_;
	std::vector<bool> mask_;
	disp_.resize(ransac_used);
	mask_.resize(ransac_used);

	for (int i = 0; i < ransac_used; ++i) {
		u = (leftPoints[i].x - ppx) / fx;
		v = (leftPoints[i].y - ppy) / fy;
		u1 = (rightPoints[i].x - ppx) / fx;

		u = u / (a31 * u + a32 * v + 1);
		v = (a21 * u + a22 * v + a23) / (a31 * u + a32 * v + 1);
		u = a11 * u + a12 * v;

		disp_[i] = u - u1;
		du_mean += u - u1;
	}
	// use 3-sgima to remove bad match
	du_mean /= ransac_used;
	for (int i = 0; i<ransac_used; ++i) {
		du_sigma += (disp_[i] - du_mean) * (disp_[i] - du_mean);
	}
	du_sigma = std::sqrt(du_sigma / ransac_used);
	for (int i = 0; i<ransac_used; ++i) {
		if (std::abs(disp_[i] - du_mean) > 3 * du_sigma)
			mask_[i] = false;
		else mask_[i] = true;
	}

	float du_min = 1e5;
	float du_max = -1e5;
	for (int i = 0; i<ransac_used; ++i) {
		if (mask_[i]) {
			if (disp_[i] < du_min) du_min = disp_[i];
			if (disp_[i] > du_max) du_max = disp_[i];
		}
	}

	IMAGE_DEPTH_LOGI("\t\t min_disp = %6.3f", du_min);
	IMAGE_DEPTH_LOGI("\t\t max_disp = %6.3f", du_max);
	
	a13 = -min_disp + 1;

#if SHOW_WINDOWS == 1
	cv::Mat showImg;
	im1.copyTo(showImg);
	for (int i = 0; i<ransac_used; i+=1) {
		if (mask_[i]) {
			putText(showImg, 
				std::to_string(int(disp_[i]+a13)), 
				Point(rightPoints[i].x, rightPoints[i].y), 
				FONT_HERSHEY_SIMPLEX, 
				0.45, 
				Scalar(255, 23, 0), 
				1, 
				1.8);
		}
	}
	ShowImg("disp_show", showImg);
#endif


#define COLOR_CONSISTENCY
#ifdef COLOR_CONSISTENCY
	ASSERT(im1.channels() == 3, "Assertion Failed with channels: %d!", im1.channels());
	// local consistency    
	// turn sparse into dense   
	cv::Mat sparse_disp(nrow, ncol, CV_8UC1, cv::Scalar(0));   
	cv::Mat sparse_colr(nrow, ncol, CV_8UC3, cv::Scalar(0, 0, 0));  
	cv::Mat sparse_mask(nrow, ncol, CV_8UC1, cv::Scalar(0));   
	// setup guide image  
	for (int i=0; i<nrow; ++i)
	{        
		for (int j=0; j<ncol; ++j)
		{            
			int offset = int((block_h*i + margin_y)*w_ + block_w*j + margin_x); 
			int offset_new = ncol * i + j;
			sparse_colr.data[3*offset_new] = im1.data[3*offset];
			sparse_colr.data[3*offset_new + 1] = im1.data[3*offset + 1];
			sparse_colr.data[3*offset_new + 2] = im1.data[3*offset + 2];
		}   
	}    
	for (int i=0; i<leftPoints.size(); ++i)
	{       
		if (mask_[i])
		{           
			int row_ = int((rightPoints[i].y - margin_y)/block_h);
			int col_ = int((rightPoints[i].x - margin_x)/block_w);
			int offset_new = ncol * row_ + col_;  
			sparse_disp.data[offset_new] = (256/max_disp)*uchar(disp_[i] + a13); 
			sparse_mask.data[offset_new] = 255;
		}
	}
	cv::Mat tmp(sparse_disp.size(), CV_8UC1, cv::Scalar(0));
	FillBlackHoles_iters(sparse_disp, sparse_colr, 1);
		
	// use a simple guided filter to filter out disparity map 
	cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls = 
		cv::ximgproc::createDisparityWLSFilterGeneric(false);
	wls->setSigmaColor(3); 
	wls->setLambda(800);
	cv::Mat sparse_disp_filtered;
	wls->filter(sparse_disp, sparse_colr, sparse_disp_filtered);
	ShowImg("sparse_disp_flt", sparse_disp_filtered);
	// check consistency error  
	uint16_t err_thrs = 8;
	sparse_disp.convertTo(sparse_disp, CV_16SC1); 
	sparse_disp_filtered.convertTo(sparse_disp_filtered, CV_16SC1); 
	cv::Mat disp_mask = cv::abs(sparse_disp_filtered - sparse_disp) <= err_thrs; 
	disp_mask = (disp_mask/255).mul(sparse_mask);
	ShowImg("disp_mask", disp_mask);
	// turn dense into sparse   
	pts1_good.resize(0);   
	pts2_good.resize(0);   
	for (int i=0; i<rightPoints.size(); ++i)
	{
		int row_ = int((rightPoints[i].y - margin_y) / block_h);
		int col_ = int((rightPoints[i].x - margin_x) / block_w);
		int offset = ncol * row_ + col_;
		if (disp_mask.data[offset])
		{          
			pts1_good.push_back(rightPoints[i]);
			pts2_good.push_back(leftPoints[i]);
		}
	}  

	std::swap(rightPoints, pts1_good);
	std::swap(leftPoints, pts2_good);

	IMAGE_DEPTH_LOGI("\t\t round 5 points: %lu", rightPoints.size());
	if (rightPoints.size() < 10) {
		im2.copyTo(im2_aligned);
		tick_stop = TICK();
		IMAGE_DEPTH_LOGI("\t RowAlign_grid DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));
		return 0;
	}

#if SHOW_WINDOWS == 1
	cv::Mat tmp_out4;
	im1.copyTo(tmp_out4);
	VisualizeOpticalFlow(tmp_out4, rightPoints, leftPoints);
	ShowImg("plk_color_consistency", tmp_out4);
#endif

	// fine tune the row align matrix  
	mat_x.create(rightPoints.size(), 5, CV_32FC1);
	vec_y.create(rightPoints.size(), 1, CV_32FC1);
	vec_beta.create(5, 1, CV_32FC1);   
	ptr_x = (float*)mat_x.data;    
	ptr_y = (float*)vec_y.data; 
	mask_accepted.resize(rightPoints.size());
	ransac_used = rightPoints.size();
	mat_x.resize(ransac_used);  
	vec_y.resize(ransac_used);  
	for (int i = 0; i < ransac_used; ++i)
	{
		v1 = (rightPoints[i].y - ppy) / fy;
		u = (leftPoints[i].x - ppx) / fx;
		v = (leftPoints[i].y - ppy) / fy;
		ptr_x[i * 5] = u;    
		ptr_x[i * 5 + 1] = v; 
		ptr_x[i * 5 + 2] = 1;
		ptr_x[i * 5 + 3] = -u * v1; 
		ptr_x[i * 5 + 4] = -v * v1;
		ptr_y[i] = v1;   
	}  
	cv::transpose(mat_x, mat_xT); 
	mat_xTx = mat_xT * mat_x; 
	
	if (cv::invert(mat_xTx, mat_xTx_inv) == 0) {
		IMAGE_DEPTH_LOGE("\t\t invert matrix failed!");
		im2.copyTo(im2_aligned);
		tick_stop = TICK();
		IMAGE_DEPTH_LOGI("\t RowAlign_grid DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));
		return 0;
	}
	
	vec_beta = mat_xT * vec_y; 
	vec_beta = mat_xTx_inv * vec_beta;   

#if SHOW_WINDOWS == 1
	// calculate average loss 
	vec_res = mat_x * vec_beta; 
	vec_res = cv::abs(vec_res - vec_y);  
	avg_res = 0;  
	for (int i = 0; i < ransac_used; ++i) 
		avg_res += vec_res.at<float>(i);
	avg_res = avg_res / ransac_used;
	IMAGE_DEPTH_LOGI("\t\t average loss= %6.3f", avg_res);
#endif

	// update row align matrix
	a21 = vec_beta.at<float>(0);  
	a22 = vec_beta.at<float>(1);  
	a23 = vec_beta.at<float>(2); 
	a31 = vec_beta.at<float>(3); 
	a32 = vec_beta.at<float>(4);

	mat_row_align.create(3, 3, CV_32F);
	mat_row_align.at<float>(0, 0) = 1;
	mat_row_align.at<float>(0, 1) = 0;
	mat_row_align.at<float>(0, 2) = 0;
	mat_row_align.at<float>(1, 0) = a21;
	mat_row_align.at<float>(1, 1) = a22;
	mat_row_align.at<float>(1, 2) = a23;
	mat_row_align.at<float>(2, 0) = a31;
	mat_row_align.at<float>(2, 1) = a32;
	mat_row_align.at<float>(2, 2) = 1;

	// update shear matrix
	h = im2.size().height; 
	w = im2.size().width;
	centers[0].x = (w - 1) / 2.0; 
	centers[0].y = 0; 
	centers[1].x = w - 1;
	centers[1].y = (h - 1) / 2.0;  
	centers[2].x = (w - 1) / 2.0;  
	centers[2].y = h - 1; 
	centers[3].x = 0; 
	centers[3].y = (h - 1) / 2.0; 
	h = im2.size().height;
	w = im2.size().width;

	for (int i = 0; i < 4; ++i){
		u = (centers[i].x - ppx) / fx; 
		v = (centers[i].y - ppy) / fy;
		centers[i].x = (u / (a31 * u + a32 * v + 1)) * fx + ppx; 
		centers[i].y = ((a21 * u + a22 * v + a23) / (a31 * u + a32 * v + 1)) * fy + ppy;
	}
	u_ = centers[1] - centers[3];
	v_ = centers[0] - centers[2]; 
	a11 = (h * h * u_.y * u_.y + w * w * v_.y * v_.y) / (h * w * (u_.y * v_.x - u_.x * v_.y)); 
	a12 = (h * h * u_.x * u_.y + w * w * v_.x * v_.y) / (h * w * (u_.x * v_.y - u_.y * v_.x)); 
	// update min and max disparity 
	du_min = 1e5;
	du_max = -1e5;
	for (int i = 0; i < ransac_used; ++i) 
	{       
		u = (leftPoints[i].x - ppx) / fx;
		v = (leftPoints[i].y - ppy) / fy;
		u1 = (rightPoints[i].x - ppx) / fx;
		u = u / (a31 * u + a32 * v + 1);
		v = (a21 * u + a22 * v + a23) / (a31 * u + a32 * v + 1);
		u = a11 * u + a12 * v; 
		float du = u - u1;  
		if (du < du_min) du_min = du; 
		if (du > du_max) du_max = du; 
	}
	IMAGE_DEPTH_LOGI("\t\t min_disp_new=%6.3f", du_min);
	IMAGE_DEPTH_LOGI("\t\t max_disp_new=%6.3f", du_max);
	
	a13 = -min_disp + 1;
#endif
	// refine the maximum disparity 
	max_disp = du_max + a13;

	cv::Mat mat_shear;
	mat_shear.create(3, 3, CV_32F);
	mat_shear.at<float>(0, 0) = a11;
	mat_shear.at<float>(0, 1) = a12;
	mat_shear.at<float>(0, 2) = a13;
	mat_shear.at<float>(1, 0) = 0;
	mat_shear.at<float>(1, 1) = 1;
	mat_shear.at<float>(1, 2) = 0;
	mat_shear.at<float>(2, 0) = 0;
	mat_shear.at<float>(2, 1) = 0;
	mat_shear.at<float>(2, 2) = 1;

	cv::Mat mat_final_transform;
	mat_final_transform = mat_shear * mat_row_align;

	cv::Mat D = cv::Mat::zeros(cv::Size(5, 1), CV_32F);
	cv::Mat map_x, map_y;
	cv::initUndistortRectifyMap(M, D, mat_final_transform, M, im2.size(), CV_32F, map_x, map_y);
	im2_aligned = cv::Mat::zeros(im2.size(), CV_8UC1) + 255; // please remove this!!!
	cv::remap(im2, im2_aligned, map_x, map_y, cv::INTER_LINEAR);

#if SHOW_WINDOWS == 1
	//std::cout << mat_final_transform << std::endl;
	ShowImg("im1_aligned", im1);
	ShowImg("im2_aligned", im2_aligned);
#endif
	
	tick_stop = TICK();
	IMAGE_DEPTH_LOGI("\t RowAlign_grid DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));

	return 1;
}


void r3d_depth_image::HistBasedFilter(cv::Mat & im_disp, float min_support, bool ignore_zero){
    cv::Mat hist;
    const int hist_size = 256;
	r3d_depth_image::Histogram(hist, im_disp, hist_size);
    //VisualizeHistogram("hist", hist);

    int idx_min, idx_max;
    float * p_hist = (float *)(hist.data);
	float sum_ = 0;

    for (int i=1; i<hist_size; ++i){
		idx_min = i;
		sum_ += p_hist[idx_min];
        if(sum_ >= min_support) break;
    }

	sum_ = 0;
	for (int i = 0; i<hist_size; ++i) {
		idx_max = 255 - i;
		sum_ += p_hist[idx_max];
		if (sum_ >= min_support) break;
	}

    cv::Mat mask_, tmp;
	cv::Mat zero_mask;
	if (ignore_zero) zero_mask = im_disp != 0;
	
    mask_ = im_disp >= idx_min;
	cv::bitwise_and(im_disp, mask_, im_disp);
	cv::bitwise_not(mask_, mask_);
	cv::bitwise_and(idx_min, mask_, tmp);
    im_disp += tmp;

	cv::threshold(im_disp, im_disp, idx_max, idx_max, cv::THRESH_TRUNC);
	
	if (ignore_zero) cv::bitwise_and(im_disp, zero_mask, im_disp);
}


void r3d_depth_image::HistBasedPeakFilter_(cv::Mat & im_disp, float min_support, int min_disp) {
	cv::Mat hist;
	const int hist_size = 256;
	r3d_depth_image::Histogram(hist, im_disp, hist_size);
	//VisualizeHistogram("hist", hist);

	int idx_max;
	float * p_hist = (float *)(hist.data);

	float sum_ = 0;

	for (int i = 0; i<255 - min_disp; ++i) {
		sum_ += p_hist[255 - i]; 
		if (sum_ >= min_support) break;
		//if (p_hist[255 - i] > min_support) break;
		idx_max = 255 - i;
	}
	
	cv::threshold(im_disp, im_disp, idx_max, 0, cv::THRESH_TOZERO_INV);
}


void r3d_depth_image::DilatedMedianBlur(
        cv::Mat & _out,
        const cv::Mat & _in,
        int radius,
        bool use_mask){
    if(_in.type() != CV_8UC1){
		IMAGE_DEPTH_LOGE("[DilatedMedianBlur]");
        exit(-1);
    }
    if (_out.size() != _in.size() ||
        _out.type() != CV_8UC1){
        _out.create(_in.size(), CV_8UC1);
    }

    int16_t t;
#define MINMAX_OP( a, b ) \
	t = a - b;\
	t = t < 0 ? 0 : t;\
    b += t; \
	a -= t;

    uint8_t * data_o = _out.data;
    uint8_t * data_i = _in.data;

    int w = _in.size().width;
    int h = _in.size().height;
    int r = radius;

    uint8_t left;
    uint8_t p0, p1, p2, p3, p4, p5, p6, p7, p8;

    // copy the border
    for(int i=0; i<r; ++i){
        for(int j=0; j<w; ++j){
            int id = i * w + j;
            data_o[id] = data_i[id];
        }
    }
    for(int i=h-r; i<h; ++i){
        for(int j=0; j<w; ++j){
            int id = i * w + j;
            data_o[id] = data_i[id];
        }
    }
    for(int i=0; i<h; ++i){
        for(int j=0; j<r; ++j){
            int id = i * w + j;
            data_o[id] = data_i[id];
        }
    }
    for(int i=0; i<h; ++i){
        for(int j=w-r; j<w; ++j){
            int id = i * w + j;
            data_o[id] = data_i[id];
        }
    }

    if (use_mask){
        for(int i=r; i<h-r; ++i){
            for(int j=r; j<w-r; ++j){
                p0 = data_i[i * w + j];
                if ( p0 ) {
                    data_o[i * w + j] = p0;
                } else {
                    p1 = data_i[i * w + j - r];
                    p2 = data_i[i * w + j + r];
                    p3 = data_i[(i - r) * w + j];
                    p4 = data_i[(i + r) * w + j];
                    p5 = data_i[(i - r) * w + j - r];
                    p6 = data_i[(i - r) * w + j + r];
                    p7 = data_i[(i + r) * w + j - r];
                    p8 = data_i[(i + r) * w + j + r];
                    // find the median value
                    MINMAX_OP(p1, p2); MINMAX_OP(p4, p5); MINMAX_OP(p7, p8);
                    MINMAX_OP(p0, p1); MINMAX_OP(p3, p4); MINMAX_OP(p6, p7);
                    MINMAX_OP(p1, p2); MINMAX_OP(p4, p5); MINMAX_OP(p7, p8);
                    MINMAX_OP(p0, p3); MINMAX_OP(p5, p8); MINMAX_OP(p4, p7);
                    MINMAX_OP(p3, p6); MINMAX_OP(p1, p4); MINMAX_OP(p2, p5);
                    MINMAX_OP(p4, p7); MINMAX_OP(p4, p2); MINMAX_OP(p6, p4);
                    MINMAX_OP(p4, p2);
                    data_o[i * w + j] = p4;
                }
            }
        }
    } else {
        for(int i=r; i<h-r; ++i){
            for(int j=r; j<w-r; ++j){
                p0 = data_i[i * w + j];
                p1 = data_i[i * w + j - r];
                p2 = data_i[i * w + j + r];
                p3 = data_i[(i - r) * w + j];
                p4 = data_i[(i + r) * w + j];
                p5 = data_i[(i - r) * w + j - r];
                p6 = data_i[(i - r) * w + j + r];
                p7 = data_i[(i + r) * w + j - r];
                p8 = data_i[(i + r) * w + j + r];
                // find the median value
                MINMAX_OP(p1, p2); MINMAX_OP(p4, p5); MINMAX_OP(p7, p8);
                MINMAX_OP(p0, p1); MINMAX_OP(p3, p4); MINMAX_OP(p6, p7);
                MINMAX_OP(p1, p2); MINMAX_OP(p4, p5); MINMAX_OP(p7, p8);
                MINMAX_OP(p0, p3); MINMAX_OP(p5, p8); MINMAX_OP(p4, p7);
                MINMAX_OP(p3, p6); MINMAX_OP(p1, p4); MINMAX_OP(p2, p5);
                MINMAX_OP(p4, p7); MINMAX_OP(p4, p2); MINMAX_OP(p6, p4);
                MINMAX_OP(p4, p2);
                data_o[i * w + j] = p4;
            }
        }
    }

#undef MINMAX_OP

}


void r3d_depth_image::DilatedMedianBlurRGB(
        cv::Mat & _out,
        const cv::Mat & _in,
        int radius
){
    if(_in.type() != CV_8UC3){
		return;
    }
    if (_out.size() != _in.size() ||
        _out.type() != CV_8UC3){
        _out.create(_in.size(), CV_8UC3);

    }

    int16_t t;
#define MINMAX_OP( a, b ) \
	t = a - b;\
	t = t < 0 ? 0 : t;\
    b += t; \
	a -= t;

    uint8_t * data_i = _in.data;

    int w = _in.size().width;
    int h = _in.size().height;
    int r = radius;

    uint8_t left;
    uint8_t p0, p1, p2, p3, p4, p5, p6, p7, p8;

    // copy the border
    for(int i=0; i<r; ++i){
        const uint8_t * data_i = _in.ptr<uint8_t>(i, 0);
        uint8_t * data_o = _out.ptr<uint8_t>(i, 0);
        for(int j=0; j<w; ++j){
            *data_o = *data_i;
            data_i ++;
            data_o ++;
            *data_o = *data_i;
            data_i ++;
            data_o ++;
            *data_o = *data_i;
            data_i ++;
            data_o ++;
        }
    }
    for(int i=h-r; i<h; ++i){
        const uint8_t * data_i = _in.ptr<uint8_t>(i, 0);
        uint8_t * data_o = _out.ptr<uint8_t>(i, 0);
        for(int j=0; j<w; ++j){
            *data_o = *data_i;
            data_i ++;
            data_o ++;
            *data_o = *data_i;
            data_i ++;
            data_o ++;
            *data_o = *data_i;
            data_i ++;
            data_o ++;
        }
    }
    for(int i=0; i<h; ++i){
        const uint8_t * data_i = _in.ptr<uint8_t>(i, 0);
        uint8_t * data_o = _out.ptr<uint8_t>(i, 0);
        for(int j=0; j<r; ++j){
            *data_o = *data_i;
            data_i ++;
            data_o ++;
            *data_o = *data_i;
            data_i ++;
            data_o ++;
            *data_o = *data_i;
            data_i ++;
            data_o ++;
        }
    }
    for(int i=0; i<h; ++i){
        const uint8_t * data_i = _in.ptr<uint8_t>(i, w-r);
        uint8_t * data_o = _out.ptr<uint8_t>(i, w-r);
        for(int j=w-r; j<w; ++j){
            *data_o = *data_i;
            data_i ++;
            data_o ++;
            *data_o = *data_i;
            data_i ++;
            data_o ++;
            *data_o = *data_i;
            data_i ++;
            data_o ++;
        }
    }

    const uint8_t *_p1, *_p2, *_p3;
    uint8_t *_p;
    const int step = 3 * r;
    const int dstep = 6 * r;

    for ( int cn = 0; cn < 3; ++cn ) {
        for ( int i = r; i < h-r; ++i ){
            _p1 = _in.ptr<uint8_t>(i - r, 0) + cn;
            _p2 = _in.ptr<uint8_t>(i, 0) + cn;
            _p3 = _in.ptr<uint8_t>(i + r, 0) + cn;
            _p = _out.ptr<uint8_t>(i, 0) + cn;
            for ( int j = r; j < w-r; ++j ){
                p0 = *(_p2 + step);
                p1 = *_p2;
                p2 = *(_p2 + dstep);
                p3 = *(_p1 + step);
                p4 = *(_p3 + step);
                p5 = *_p1;
                p6 = *(_p1 + dstep);
                p7 = *_p3;
                p8 = *(_p3 + dstep);
                // find the median value
                MINMAX_OP(p1, p2); MINMAX_OP(p4, p5); MINMAX_OP(p7, p8);
                MINMAX_OP(p0, p1); MINMAX_OP(p3, p4); MINMAX_OP(p6, p7);
                MINMAX_OP(p1, p2); MINMAX_OP(p4, p5); MINMAX_OP(p7, p8);
                MINMAX_OP(p0, p3); MINMAX_OP(p5, p8); MINMAX_OP(p4, p7);
                MINMAX_OP(p3, p6); MINMAX_OP(p1, p4); MINMAX_OP(p2, p5);
                MINMAX_OP(p4, p7); MINMAX_OP(p4, p2); MINMAX_OP(p6, p4);
                MINMAX_OP(p4, p2);
                *_p = p4;
                _p1 += 3;
                _p2 += 3;
                _p3 += 3;
                _p += 3;
            }
        }
    }
#undef MINMAX_OP
#undef CV_FAST_CAST_8U
}


void r3d_depth_image::FillBlackHoles_new(cv::Mat & io_, const cv::Mat & guide, bool dilation) {
	if (dilation) {
		cv::Mat tmp1, tmp2, mask;
		mask = io_ == 0;
		Dilate_v3_fastest(tmp1, io_, guide, 8, 4, 255);
		Dilate_v3_fastest(tmp2, tmp1, guide, 8, 4, 255);
		cv::bitwise_and(tmp2, mask, tmp2);
		cv::bitwise_or(io_, tmp2, io_);
	}
	// search from the center to the first nonzero value in all 4 ways.
	uint8_t * data = io_.data;
	uint8_t * data_g = guide.data;

	int h = io_.size().height;
	int w = io_.size().width;

	int offset;
	int depth[4];
	int color[5][3];
	bool is_found[4];
	int end_idx;
	int end_row;
	int s;
	int i, j;
	for(j = 0; j<w; ++j){
		// set the column search cache
		for(i = 0; i<h; ++i){ // row-wise is more cache friendly!
			offset = i*w + j;
			if (!data[offset]) { // this pixel is within a black hole
				// step 1: find row range of the black hole
				s = offset - w;
				if (offset < w){
					is_found[2] = false;
				}
				else {
					depth[2] = data[s];
					color[2][0] = data_g[s * 3];
					color[2][1] = data_g[s * 3 + 1];
					color[2][2] = data_g[s * 3 + 2];
					is_found[2] = true;
				}
				// search the range tail
				is_found[3] = false;
				end_idx = j + (h - 1) * w;
				end_row = i;
				for (s = offset + w; s <= end_idx; s += w) {
					if (data[s]){
						depth[3] = data[s];
						color[3][0] = data_g[s * 3];
						color[3][1] = data_g[s * 3 + 1];
						color[3][2] = data_g[s * 3 + 2];
						is_found[3] = true;
						break;
					}
					end_row++;
				}
				// step 2: fill in this column of black holes
				for (; i <= end_row; ++i){
					offset = i * w + j;
					// get color of this black pixel in guide image
					color[4][0] = data_g[offset * 3];
					color[4][1] = data_g[offset * 3 + 1];
					color[4][2] = data_g[offset * 3 + 2];
					// search left
					is_found[0] = false;
					end_idx = offset - j;
					for (s = offset - 1; s >= end_idx; --s) {
						if (data[s]) {
							depth[0] = data[s];
							color[0][0] = data_g[s * 3];
							color[0][1] = data_g[s * 3 + 1];
							color[0][2] = data_g[s * 3 + 2];
							is_found[0] = true;
							break;
						}
					}
					// search right
					is_found[1] = false;
					end_idx = offset - j + w - 1;
					for (int s = offset + 1; s <= end_idx; ++s) {
						if (data[s]) {
							depth[1] = data[s];
							color[1][0] = data_g[s * 3];
							color[1][1] = data_g[s * 3 + 1];
							color[1][2] = data_g[s * 3 + 2];
							is_found[1] = true;
							break;
						}
					}
					// interpolate according to color similarity
					int sum_ = 0;
					int sum_depth = 0;
					for (int s = 0; s < 4; ++s) {
						if (is_found[s]) {
							int w_ = std::abs(color[4][0] - color[s][0]) +
								std::abs(color[4][1] - color[s][1]) +
								std::abs(color[4][2] - color[s][2]);
							w_ = (255 * 255) / (w_ + 1);
							sum_ += w_;
							sum_depth += w_ * depth[s];
						}
					}
					if (sum_ > 0) data[offset] = (uint8_t)(sum_depth / sum_);
				}
				--i;
			}
		}
	}
}


void r3d_depth_image::FillBlackHolesSmallRegion(cv::Mat & io_, const cv::Mat & guide, int iters) {


	// search from the center to the first nonzero value in all 4 ways.
	uint8_t * data = io_.data;
	uint8_t * data_g = guide.data;

	if (!io_.isContinuous()) {
		IMAGE_DEPTH_LOGE("FillBlackHoles_iters assertion error: io_ is not continuous!");
	}
	if (!guide.isContinuous()) {
		IMAGE_DEPTH_LOGE("FillBlackHoles_iters assertion error: guide is not continuous!");
	}

	int h = io_.size().height;
	int w = io_.size().width;

	iters = 3;
	for (int k = 0; k<iters; k++)
	{
		int offset = 0;
		int depth_[4];
		int color_[5][3];
		bool is_found[4];


		if (k == 0)
		{
			int step[4];

			for (int i = 0; i<h; ++i) {
				for (int j = 0; j<w; ++j) {
					if (!data[offset]) { // this pixel is within a black hole
						color_[4][0] = data_g[offset * 3];
						color_[4][1] = data_g[offset * 3 + 1];
						color_[4][2] = data_g[offset * 3 + 2];
						is_found[0] = false;
						int end_idx = offset - j;

						int stp_ = 0;

						for (int s = offset - 1; s >= end_idx; --s) { // search left
							stp_++;
							if (data[s]) { // meet a nonzero value
								depth_[0] = data[s];
								color_[0][0] = data_g[s * 3];
								color_[0][1] = data_g[s * 3 + 1];
								color_[0][2] = data_g[s * 3 + 2];
								is_found[0] = true;
								step[0] = stp_;
								break;
							}
						}
						is_found[1] = false;
						end_idx = offset - j + w - 1;
						stp_ = 0;
						for (int s = offset + 1; s <= end_idx; ++s) { // search right
							stp_++;
							if (data[s]) { // meet a nonzero value
								depth_[1] = data[s];
								color_[1][0] = data_g[s * 3];
								color_[1][1] = data_g[s * 3 + 1];
								color_[1][2] = data_g[s * 3 + 2];
								is_found[1] = true;
								step[1] = stp_;

								break;
							}
						}
						is_found[2] = false;
						end_idx = j;
						stp_ = 0;
						for (int s = offset - w; s >= end_idx; s -= w) { // search up
							stp_++;
							if (data[s]) { // meet a nonzero value
								depth_[2] = data[s];
								color_[2][0] = data_g[s * 3];
								color_[2][1] = data_g[s * 3 + 1];
								color_[2][2] = data_g[s * 3 + 2];
								is_found[2] = true;
								step[2] = stp_;
								break;
							}
						}
						is_found[3] = false;
						end_idx = j + (h - 1) * w;
						stp_ = 0;
						for (int s = offset + w; s <= end_idx; s += w) { // search down
							stp_++;
							if (data[s]) { // meet a nonzero value
								depth_[3] = data[s];
								color_[3][0] = data_g[s * 3];
								color_[3][1] = data_g[s * 3 + 1];
								color_[3][2] = data_g[s * 3 + 2];
								is_found[3] = true;
								step[3] = stp_;
								break;
							}
						}

						if (is_found[0] && is_found[1] && is_found[2] && is_found[3])
						{
							/*int color_flag = false;
							int sum_ = 0, num = 0;
							for (int s = 0; s < 4; ++s) {
							int w_ = std::abs(color_[4][0] - color_[s][0]) +
							std::abs(color_[4][1] - color_[s][1]) +
							std::abs(color_[4][2] - color_[s][2]);

							if (w_ < 5)
							{
							color_flag = true;
							sum_ += depth_[s];
							num++;
							}
							}

							if (color_flag)
							{
							data[offset] = (uint8_t)(sum_ / num);
							}
							else
							{
							if (step[0] + step[1]<35 && step[2] + step[3]<35)
							{
							int value = 256;
							for (int s = 0; s<4; ++s) {
							if (depth_[s]<value)
							value = depth_[s];
							}
							data[offset] = value;
							}
							}*/

							if (step[0] + step[1]<35 && step[2] + step[3]<35)
							{
								int value = 256;
								for (int s = 0; s<4; ++s) {
									if (depth_[s]<value)
										value = depth_[s];
								}
								data[offset] = value;
							}
						}
					}
					++offset;
				}
			}

		}
		else if (k == 1)
		{
			for (int i = 0; i<h; ++i) {
				for (int j = 0; j<w; ++j) {
					if (!data[offset]) { // this pixel is within a black hole
						color_[4][0] = data_g[offset * 3];
						color_[4][1] = data_g[offset * 3 + 1];
						color_[4][2] = data_g[offset * 3 + 2];
						is_found[0] = false;
						int end_idx = offset - j;
						for (int s = offset - 1; s >= end_idx; --s) { // search left
							if (data[s]) { // meet a nonzero value
								depth_[0] = data[s];
								color_[0][0] = data_g[s * 3];
								color_[0][1] = data_g[s * 3 + 1];
								color_[0][2] = data_g[s * 3 + 2];
								is_found[0] = true;
								break;
							}
						}
						is_found[1] = false;
						end_idx = offset - j + w - 1;
						for (int s = offset + 1; s <= end_idx; ++s) { // search right
							if (data[s]) { // meet a nonzero value
								depth_[1] = data[s];
								color_[1][0] = data_g[s * 3];
								color_[1][1] = data_g[s * 3 + 1];
								color_[1][2] = data_g[s * 3 + 2];
								is_found[1] = true;
								break;
							}
						}
						is_found[2] = false;
						end_idx = j;
						for (int s = offset - w; s >= end_idx; s -= w) { // search up
							if (data[s]) { // meet a nonzero value
								depth_[2] = data[s];
								color_[2][0] = data_g[s * 3];
								color_[2][1] = data_g[s * 3 + 1];
								color_[2][2] = data_g[s * 3 + 2];
								is_found[2] = true;
								break;
							}
						}
						is_found[3] = false;
						end_idx = j + (h - 1) * w;
						for (int s = offset + w; s <= end_idx; s += w) { // search down
							if (data[s]) { // meet a nonzero value
								depth_[3] = data[s];
								color_[3][0] = data_g[s * 3];
								color_[3][1] = data_g[s * 3 + 1];
								color_[3][2] = data_g[s * 3 + 2];
								is_found[3] = true;
								break;
							}
						}
						// interpolate according to color similarity
						//if (is_found[0] && is_found[1] && is_found[2] && is_found[3])
						{
							int sum_ = 0;
							int sum_depth = 0;
							for (int s = 0; s<4; ++s) {
								if (is_found[s]) {
									int w_ = std::abs(color_[4][0] - color_[s][0]) +
										std::abs(color_[4][1] - color_[s][1]) +
										std::abs(color_[4][2] - color_[s][2]);
									if (w_ < 10)
									{
										w_ = (255 * 255) / (w_ + 1);
										sum_ += w_;
										sum_depth += w_ * depth_[s];
									}
								}
							}
							if (sum_ > 0) data[offset] = (uint8_t)(sum_depth / sum_);

						}
					}
					++offset;
				}
			}
		}
		else
		{
			for (int i = 0; i<h; ++i) {
				for (int j = 0; j<w; ++j) {
					if (!data[offset]) { // this pixel is within a black hole
						color_[4][0] = data_g[offset * 3];
						color_[4][1] = data_g[offset * 3 + 1];
						color_[4][2] = data_g[offset * 3 + 2];
						is_found[0] = false;
						int end_idx = offset - j;
						for (int s = offset - 1; s >= end_idx; --s) { // search left
							if (data[s]) { // meet a nonzero value
								depth_[0] = data[s];
								color_[0][0] = data_g[s * 3];
								color_[0][1] = data_g[s * 3 + 1];
								color_[0][2] = data_g[s * 3 + 2];
								is_found[0] = true;
								break;
							}
						}
						is_found[1] = false;
						end_idx = offset - j + w - 1;
						for (int s = offset + 1; s <= end_idx; ++s) { // search right
							if (data[s]) { // meet a nonzero value
								depth_[1] = data[s];
								color_[1][0] = data_g[s * 3];
								color_[1][1] = data_g[s * 3 + 1];
								color_[1][2] = data_g[s * 3 + 2];
								is_found[1] = true;
								break;
							}
						}
						is_found[2] = false;
						end_idx = j;
						for (int s = offset - w; s >= end_idx; s -= w) { // search up
							if (data[s]) { // meet a nonzero value
								depth_[2] = data[s];
								color_[2][0] = data_g[s * 3];
								color_[2][1] = data_g[s * 3 + 1];
								color_[2][2] = data_g[s * 3 + 2];
								is_found[2] = true;
								break;
							}
						}
						is_found[3] = false;
						end_idx = j + (h - 1) * w;
						for (int s = offset + w; s <= end_idx; s += w) { // search down
							if (data[s]) { // meet a nonzero value
								depth_[3] = data[s];
								color_[3][0] = data_g[s * 3];
								color_[3][1] = data_g[s * 3 + 1];
								color_[3][2] = data_g[s * 3 + 2];
								is_found[3] = true;
								break;
							}
						}
						// interpolate according to color similarity
						int sum_ = 0;
						int sum_depth = 0;
						for (int s = 0; s<4; ++s) {
							if (is_found[s]) {
								int w_ = std::abs(color_[4][0] - color_[s][0]) +
									std::abs(color_[4][1] - color_[s][1]) +
									std::abs(color_[4][2] - color_[s][2]);
								w_ = (255 * 255) / (w_ + 1);
								sum_ += w_;
								sum_depth += w_ * depth_[s];
							}
						}
						if (sum_ > 0) data[offset] = (uint8_t)(sum_depth / sum_);
					}
					++offset;
				}
			}
		}
	}


}


void r3d_depth_image::DumpImage(const cv::Mat & im_,
               const cv::String & path,
               const cv::String & postfix,
               const cv::String & uid
){
#ifdef _WIN32
	cv::String cmd(path.c_str());
	std::replace(cmd.begin(), cmd.end(), '/', '\\');
	cmd = "md " + cmd;
	system(cmd.c_str());
#else
	cv::String cmd(path.c_str());
	cmd = "mkdir -p " + cmd;
	system(cmd.c_str());
#endif
	if (postfix.empty())
		cv::imwrite(path + "/" + uid + ".png", im_);
	else
		cv::imwrite(path + "/" + uid + "_" + postfix + ".png", im_);
}


void r3d_depth_image::Disparity2Depth(cv::Mat &disp, float & min_, float & max_) {
	float min_t = min_;
	float max_t = max_;

	cv::Mat disp_f32;
    disp.convertTo(disp_f32, CV_32FC1);
	disp_f32 = 255*256 / (disp_f32 + 1);
	
	cv::Mat t_;
	cv::reduce(disp_f32, t_, 0, REDUCE_MIN, -1);
	cv::reduce(t_, t_, 1, REDUCE_MIN, -1);
	min_ = t_.at<float>(0, 0);

	cv::reduce(disp_f32, t_, 0, REDUCE_MAX, -1);
	cv::reduce(t_, t_, 1, REDUCE_MAX, -1);
	max_ = t_.at<float>(0, 0);

    cv::normalize(disp_f32, disp_f32, min_t, max_t, cv::NORM_MINMAX, -1);
    disp_f32.convertTo(disp, CV_8UC1);
}


void r3d_depth_image::Depth2Disparity(cv::Mat &depth, float min_, float max_) {
	cv::Mat disp_f32;
	depth.convertTo(disp_f32, CV_32FC1);
	cv::normalize(disp_f32, disp_f32, min_, max_, cv::NORM_MINMAX, -1);

	disp_f32 = 255 * 256 / disp_f32 - 1;
	disp_f32.convertTo(depth, CV_8UC1);
}


void r3d_depth_image::ChangeSaturation(cv::Mat &in_out, int degree){
	if (in_out.type() != CV_8UC3) {
		IMAGE_DEPTH_LOGE("Assertion Failure in [ChangeSaturation]!");
		FAIL_EXIT(IMAGE_DEPTH_LOGIC_ERROR);
	}
#define USE_OMP 1
#if USE_OMP == 1
#define OMP_THREADS 4
	int n_row = in_out.size().height;
	int pitch = (n_row + OMP_THREADS - 1) / OMP_THREADS;
	int n_col = in_out.size().width;
	
	float Increment = std::min(std::max(degree, -100), 100) / 100.0F;
	if (Increment >= 0) {
#pragma omp parallel for num_threads(OMP_THREADS)
		for (int idx = 0; idx < OMP_THREADS; idx++) {
			cv::MatIterator_<cv::Vec3b> p_cur, p_end;
			p_cur = in_out.begin<cv::Vec3b>();
			p_cur += idx * pitch * n_col;
			p_end = in_out.begin<cv::Vec3b>();
			p_end += std::min(n_row, (idx + 1)*pitch)*n_col;

			int t1, t2, t3, minVal, maxVal;
			float delta, L, S, alpha;
			for (; p_cur != p_end; p_cur++) {
				t1 = (*p_cur)[0];
				t2 = (*p_cur)[1];
				t3 = (*p_cur)[2];
				minVal = std::min(t1 < t2 ? t1 : t2, t3);
				maxVal = std::max(t1 > t2 ? t1 : t2, t3);
				delta = maxVal - minVal + 1e-3;
				L = (maxVal + minVal) / 2.0;
				S = 0.5 * delta / (std::fmin(L, 255 - L) + 1e-3);
				alpha = std::max(S, 1 - Increment);
				float decay = (255 - L) / 255.0;
				decay *= decay;
				alpha = (1.0 / alpha - 1)*decay;
				(*p_cur)[0] = t1 + (t1 - L)*alpha;
				(*p_cur)[1] = t2 + (t2 - L)*alpha;
				(*p_cur)[2] = t3 + (t3 - L)*alpha;
			}
		}
	}
	else {
		// not implemented
		IMAGE_DEPTH_LOGE("Interface unimplemented: [ChangeSaturation]");
		FAIL_EXIT(IMAGE_DEPTH_UNIMPL_ERROR);
	}
	
#else
	cv::MatIterator_<cv::Vec3b> p_cur, p_end;
	p_cur = in_out.begin<cv::Vec3b>();
	p_end = in_out.end<cv::Vec3b>();
	float Increment = std::min(std::max(degree, -100), 100) / 100.0F;
	int t1, t2, t3, minVal, maxVal;
	float delta, L, S, alpha;
	if (Increment >= 0) {
		cv::MatIterator_<cv::Vec3b> p_cur, p_end;
		p_cur = in_out.begin<cv::Vec3b>();
		p_end = in_out.end<cv::Vec3b>();
		float Increment = std::min(std::max(degree, -100), 100) / 100.0F;
		int t1, t2, t3, minVal, maxVal;
		float delta, L, S, alpha;
		if (Increment >= 0) {
			for (; p_cur != p_end; p_cur++) {
				t1 = (*p_cur)[0];
				t2 = (*p_cur)[1];
				t3 = (*p_cur)[2];
				minVal = std::min(t1 < t2 ? t1 : t2, t3);
				maxVal = std::max(t1 > t2 ? t1 : t2, t3);
				delta = maxVal - minVal + 1e-3;
				L = (maxVal + minVal) / 2.0;
				S = 0.5 * delta / (std::fmin(L, 255 - L) + 1e-3);
				alpha = std::max(S, 1 - Increment);
				float decay = (255 - L) / 255.0;
				decay *= decay;
				alpha = (1.0 / alpha - 1)*decay;
				(*p_cur)[0] = t1 + (t1 - L)*alpha;
				(*p_cur)[1] = t2 + (t2 - L)*alpha;
				(*p_cur)[2] = t3 + (t3 - L)*alpha;
			}
		}
	}
	else {
		for (; p_cur != p_end; p_cur++) {
			t1 = (*p_cur)[0];
			t2 = (*p_cur)[1];
			t3 = (*p_cur)[2];
			minVal = std::min(std::min(t1, t2), t3);
			maxVal = std::max(std::max(t1, t2), t3);
			L = (maxVal + minVal) / 2.0;
			(*p_cur)[0] = L + (t1 - L)*(1 + Increment);
			(*p_cur)[1] = L + (t2 - L)*(1 + Increment);
			(*p_cur)[2] = L + (t3 - L)*(1 + Increment);
		}
	}
#endif
}


void r3d_depth_image::removeWhiteEdge(cv::Mat & input)
{
	if (input.type() != CV_8UC1)
		return;
	int w = input.cols, h = input.rows;

	for (int i = 0; i < h; i++)
	{
		uchar* ptr_ = input.ptr<uchar>(i);
		for (int j = 0; j < 16; j++)//16
		{
			if (ptr_[j] > 125)
			{
				ptr_[j] = 0;
			}
		}
	}
}


void r3d_depth_image::removeTextureless(cv::Mat & disp, cv::Mat &guided)
{
	cv::Mat mask_;
	cv::threshold(disp, mask_, 130, 255, cv::THRESH_BINARY);

	ShowImg("white", mask_);

	int w = disp.cols, h = disp.rows;

	std::vector<std::vector<cv::Point>> contours;
	
	cv::findContours(mask_, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE, cv::Point());
	size_t size = contours.size();

	for (int i = 0; i < size; i++)
	{
		if(cv::contourArea(contours[i])<100)
		{
			cv::drawContours(disp, contours, i, cv::Scalar(0), -1, 1);
		}
		else
		{
			if (cv::contourArea(contours[i]) < 15000)
			{
				cv::Mat roi = guided(cv::boundingRect(contours[i])).clone();
				cv::Mat mean_, sd;
				cv::meanStdDev(roi, mean_, sd);
				double std_ = sd.at<double>(0, 0);

				if (std_ < 10)
				{
					cv::drawContours(disp, contours, i, cv::Scalar(0), -1, 1);
				}
			}
		}
	}
}


void r3d_depth_image::removeSpot(cv::Mat &disp)
{
	cv::Mat thresh;
	cv::threshold(disp, thresh, 0, 255, cv::THRESH_BINARY);
	std::vector<std::vector<cv::Point>> contours;

	cv::findContours(thresh, contours, cv::RETR_TREE, cv::CHAIN_APPROX_NONE, cv::Point()); //RETR_EXTERNAL

	cv::Mat mul(thresh.size(), CV_8UC1, cv::Scalar(1));
	size_t size_ = contours.size();

	for (int i = 0; i < size_; i++)
	{
		int area = cv::contourArea(contours[i]);
		cv::Rect rt = cv::boundingRect(contours[i]);
		if (area < 300) // 300
		{
			if(disp.at<uchar>(rt.y+(rt.height>>1),rt.x+(rt.width>>1))>80) // > 0
				cv::drawContours(mul, contours, i, cv::Scalar(0), -1, 1);
		}
	}

	
	disp = disp.mul(mul);
}


void r3d_depth_image::FillBlackHoles_iters(cv::Mat & io_, const cv::Mat & guide, int iters) {
	// search from the center to the first nonzero value in all 4 ways.
	uint8_t * data = io_.data;
	uint8_t * data_g = guide.data;

	if (!io_.isContinuous()) {
		IMAGE_DEPTH_LOGE("FillBlackHoles_iters assertion error: io_ is not continuous!");
	}
	if (!guide.isContinuous()) {
		IMAGE_DEPTH_LOGE("FillBlackHoles_iters assertion error: guide is not continuous!");
	}

	int h = io_.size().height;
	int w = io_.size().width;

	int iters_ = iters - 1;
	for (int k = 0; k < iters; k++)
	{
		int offset = 0;
		int depth_[4];
		int color_[5][3];
		bool is_found[4];

		if (k<iters_)
		{
			for (int i = 0; i<h; ++i) {
				for (int j = 0; j<w; ++j) {
					if (!data[offset]) { // this pixel is within a black hole
						color_[4][0] = data_g[offset * 3];
						color_[4][1] = data_g[offset * 3 + 1];
						color_[4][2] = data_g[offset * 3 + 2];
						is_found[0] = false;
						int end_idx = offset - j;
						for (int s = offset - 1; s >= end_idx; --s) { // search left
							if (data[s]) { // meet a nonzero value
								depth_[0] = data[s];
								color_[0][0] = data_g[s * 3];
								color_[0][1] = data_g[s * 3 + 1];
								color_[0][2] = data_g[s * 3 + 2];
								is_found[0] = true;
								break;
							}
						}
						is_found[1] = false;
						end_idx = offset - j + w - 1;
						for (int s = offset + 1; s <= end_idx; ++s) { // search right
							if (data[s]) { // meet a nonzero value
								depth_[1] = data[s];
								color_[1][0] = data_g[s * 3];
								color_[1][1] = data_g[s * 3 + 1];
								color_[1][2] = data_g[s * 3 + 2];
								is_found[1] = true;
								break;
							}
						}
						is_found[2] = false;
						end_idx = j;
						for (int s = offset - w; s >= end_idx; s -= w) { // search up
							if (data[s]) { // meet a nonzero value
								depth_[2] = data[s];
								color_[2][0] = data_g[s * 3];
								color_[2][1] = data_g[s * 3 + 1];
								color_[2][2] = data_g[s * 3 + 2];
								is_found[2] = true;
								break;
							}
						}
						is_found[3] = false;
						end_idx = j + (h - 1) * w;
						for (int s = offset + w; s <= end_idx; s += w) { // search down
							if (data[s]) { // meet a nonzero value
								depth_[3] = data[s];
								color_[3][0] = data_g[s * 3];
								color_[3][1] = data_g[s * 3 + 1];
								color_[3][2] = data_g[s * 3 + 2];
								is_found[3] = true;
								break;
							}
						}
						// interpolate according to color similarity
						//if (is_found[0] && is_found[1] && is_found[2] && is_found[3])
						{
							int sum_ = 0;
							int sum_depth = 0;
							for (int s = 0; s<4; ++s) {
								if (is_found[s]) {
									int w_ = std::abs(color_[4][0] - color_[s][0]) +
										std::abs(color_[4][1] - color_[s][1]) +
										std::abs(color_[4][2] - color_[s][2]);
									if (w_ < 10)
									{
										w_ = (255 * 255) / (w_ + 1);
										sum_ += w_;
										sum_depth += w_ * depth_[s];
									}
								}
							}
							if (sum_ > 0) data[offset] = (uint8_t)(sum_depth / sum_);

						}
					}
					++offset;
				}
			}
		}
		else
		{
			for (int i = 0; i<h; ++i) {
				for (int j = 0; j<w; ++j) {
					if (!data[offset]) { // this pixel is within a black hole
						color_[4][0] = data_g[offset * 3];
						color_[4][1] = data_g[offset * 3 + 1];
						color_[4][2] = data_g[offset * 3 + 2];
						is_found[0] = false;
						int end_idx = offset - j;
						for (int s = offset - 1; s >= end_idx; --s) { // search left
							if (data[s]) { // meet a nonzero value
								depth_[0] = data[s];
								color_[0][0] = data_g[s * 3];
								color_[0][1] = data_g[s * 3 + 1];
								color_[0][2] = data_g[s * 3 + 2];
								is_found[0] = true;
								break;
							}
						}
						is_found[1] = false;
						end_idx = offset - j + w - 1;
						for (int s = offset + 1; s <= end_idx; ++s) { // search right
							if (data[s]) { // meet a nonzero value
								depth_[1] = data[s];
								color_[1][0] = data_g[s * 3];
								color_[1][1] = data_g[s * 3 + 1];
								color_[1][2] = data_g[s * 3 + 2];
								is_found[1] = true;
								break;
							}
						}
						is_found[2] = false;
						end_idx = j;
						for (int s = offset - w; s >= end_idx; s -= w) { // search up
							if (data[s]) { // meet a nonzero value
								depth_[2] = data[s];
								color_[2][0] = data_g[s * 3];
								color_[2][1] = data_g[s * 3 + 1];
								color_[2][2] = data_g[s * 3 + 2];
								is_found[2] = true;
								break;
							}
						}
						is_found[3] = false;
						end_idx = j + (h - 1) * w;
						for (int s = offset + w; s <= end_idx; s += w) { // search down
							if (data[s]) { // meet a nonzero value
								depth_[3] = data[s];
								color_[3][0] = data_g[s * 3];
								color_[3][1] = data_g[s * 3 + 1];
								color_[3][2] = data_g[s * 3 + 2];
								is_found[3] = true;
								break;
							}
						}
						// interpolate according to color similarity
						int sum_ = 0;
						int sum_depth = 0;
						for (int s = 0; s<4; ++s) {
							if (is_found[s]) {
								int w_ = std::abs(color_[4][0] - color_[s][0]) +
									std::abs(color_[4][1] - color_[s][1]) +
									std::abs(color_[4][2] - color_[s][2]);
								w_ = (255 * 255) / (w_ + 1);
								sum_ += w_;
								sum_depth += w_ * depth_[s];
							}
						}
						if (sum_ > 0) data[offset] = (uint8_t)(sum_depth / sum_);
					}
					++offset;
				}
			}
		}
	}


}


void r3d_depth_image::StringSplit(const string& s, vector<string>& tokens, const string& delimiters = " ") {
	string::size_type lastPos = s.find_first_not_of(delimiters, 0);
	string::size_type pos = s.find_first_of(delimiters, lastPos);
	while (string::npos != pos || string::npos != lastPos) {
		tokens.push_back(s.substr(lastPos, pos - lastPos));//use emplace_back after C++11
		lastPos = s.find_first_not_of(delimiters, pos);
		pos = s.find_first_of(delimiters, lastPos);
	}
}


std::string& r3d_depth_image::StringTrim(std::string &s) {
	if (s.empty()) return s;
	s.erase(0, s.find_first_not_of(" "));
	s.erase(s.find_last_not_of(" ") + 1);
	return s;
}


void r3d_depth_image::ChopCorners(cv::Mat & io_, int size) {
	if (!io_.isContinuous()) {
		IMAGE_DEPTH_LOGE("ASSERTION FAILED: ChopRightCorners requires cnotinuous memory of input!");
		exit(-1);
	}
	uchar * p = io_.data;
	int w = io_.size().width;
	int h = io_.size().height;
	// right side
	for (int i = 0; i < size; ++i) {
		for (int j = w - size + i; j < w; ++j) {
			p[i*w + j] = 0;
		}
	}

	for (int i = h - size; i < h; ++i) {
		for (int j = w - size + h - i; j < w; ++j) {
			p[i*w + j] = 0;
		}
	}
	// left side
	for (int i = 0; i < size; ++i) {
		for (int j = 0; j < size - i; ++j) {
			p[i*w + j] = 0;
		}
	}

	for (int i = h - size; i < h; ++i) {
		for (int j = 0; j < size - h + i; ++j) {
			p[i*w + j] = 0;
		}
	}
}


void r3d_depth_image::CropBall(cv::Mat & io_, float ratio) {
	if (!io_.isContinuous()) {
		IMAGE_DEPTH_LOGE("ASSERTION FAILED: ChopRightCorners requires cnotinuous memory of input!");
		exit(-1);
	}
	uchar * p = io_.data;
	int w = io_.size().width;
	int h = io_.size().height;
	// any pixels beyond the inner circle is regarded as bad ones
	int sw = w / 2;
	int sh = h / 2;
	int r2 = (1 - ratio) * (1 - ratio) * sw * sw;
	int y2 = 0;
	for (int i = 0; i < sh; ++i) {
		y2 = (i - sh)*(i - sh);
		for (int j = 0; j < sw; ++j) {
			if (y2 + (j - sw)*(j - sw) > r2) {
				p[i*w + j] = 0;
				p[i*w + w - 1 - j] = 0;
				p[(h - 1 - i)*w + j] = 0;
				p[(h - 1 - i)*w + w - 1 - j] = 0;
			}
		}
	}
}


void r3d_depth_image::FillBlackHolesSimple(
	cv::Mat & im_rgb, 
	cv::Mat & depth) {
	// search from the center to the first nonzero value in all 4 ways.
	if (!im_rgb.isContinuous()) {
		IMAGE_DEPTH_LOGE("[FillBlackHolesSimple] im_rgb discontinuous memory");
		FAIL_EXIT(-1);
	}

	// check the dimension of input
	if (im_rgb.type() != CV_8UC3) {
		IMAGE_DEPTH_LOGE("[FillBlackHolesSimple] im_rgb has invalid type");
		FAIL_EXIT(-1);
	}

	// check input mask
	if (!depth.isContinuous()) {
		IMAGE_DEPTH_LOGE("[FillBlackHolesSimple] depth discontinuous memory");
		FAIL_EXIT(-1);
	}

	if (depth.type() != CV_32FC1) {
		IMAGE_DEPTH_LOGE("[FillBlackHolesSimple] depth has invalid type");
		FAIL_EXIT(-1);
	}

	if (depth.size() != im_rgb.size()) {
		IMAGE_DEPTH_LOGE("[FillBlackHolesSimple] size of depth mismatch that of im_rgb");
		FAIL_EXIT(-1);
	}

	uint8_t * pC = im_rgb.data;
	float * pD = (float*)(depth.data);

	int h = im_rgb.size().height;
	int w = im_rgb.size().width;

	int offset;
	int color[4][3];
	float z[4]; // depth info of neighbor points
	int dxy[4]; // delta xy info of neighbor points
	bool is_found[4];
	int end_idx;
	int end_row;
	int s;
	int i, j;

	for (j = 0; j<w; ++j) {
		// set the column search cache
		for (i = 0; i<h; ++i) { // row-wise is more cache friendly!
			offset = i*w + j;
			if (!pD[offset]) { // this pixel is within a black hole
				// step 1: find row range of the black hole
				s = offset - w;
				if (s < 0) {
					is_found[2] = false;
				}
				else {
					color[2][0] = pC[s * 3];
					color[2][1] = pC[s * 3 + 1];
					color[2][2] = pC[s * 3 + 2];
					is_found[2] = true;
					dxy[2] = s;
					z[2] = pD[s];
				}
				// search the range tail
				is_found[3] = false;
				end_idx = j + (h - 1) * w;
				end_row = i;
				for (s = offset + w; s <= end_idx; s += w) {
					if (pD[s]) {
						color[3][0] = pC[s * 3];
						color[3][1] = pC[s * 3 + 1];
						color[3][2] = pC[s * 3 + 2];
						is_found[3] = true;
						dxy[3] = s;
						z[3] = pD[s];
						break;
					}
					end_row++;
				}
				// step 2: fill in this column of black holes
				for (; i <= end_row; ++i) {
					offset = i * w + j;
					// search left
					is_found[0] = false;
					end_idx = offset - j;
					for (s = offset - 1; s >= end_idx; --s) {
						if (pD[s]) {
							color[0][0] = pC[s * 3];
							color[0][1] = pC[s * 3 + 1];
							color[0][2] = pC[s * 3 + 2];
							is_found[0] = true;
							dxy[0] = s;
							z[0] = pD[s];
							break;
						}
					}
					// search right
					is_found[1] = false;
					end_idx = offset - j + w - 1;
					for (int s = offset + 1; s <= end_idx; ++s) {
						if (pD[s]) {
							color[1][0] = pC[s * 3];
							color[1][1] = pC[s * 3 + 1];
							color[1][2] = pC[s * 3 + 2];
							is_found[1] = true;
							dxy[1] = s;
							z[1] = pD[s];
							break;
						}
					}
					// interpolate according to color similarity
					float sum_w = 0;
					float sum_r = 0;
					float sum_g = 0;
					float sum_b = 0;
					float sum_d = 0;
					int x0 = offset % w;
					int y0 = offset / w;
					float w_ = 0;

					for (int s = 0; s < 4; ++s) {
						if (is_found[s]) {
							int x_ = dxy[s] % w;
							int y_ = dxy[s] / w;
							w_ = z[s] / (std::fabs(x_ - x0) + std::fabs(y_ - y0));
							sum_w += w_;
							sum_r += w_ * color[s][0];
							sum_g += w_ * color[s][1];
							sum_b += w_ * color[s][2];
							sum_d += w_ * z[s];
						}
					}
					if (sum_w > 0) {
						pC[3 * offset] = (uint8_t)(sum_r / sum_w);
						pC[3 * offset + 1] = (uint8_t)(sum_g / sum_w);
						pC[3 * offset + 2] = (uint8_t)(sum_b / sum_w);
						pD[offset] = sum_d / sum_w;
					}
				}
				--i;
			}
		}
	}
}


void r3d_depth_image::ShowImg(cv::String name, const cv::Mat & im) {
#if SHOW_WINDOWS == 1
	if (im.size().width < 256) {
		cv::Mat tmp_;
		cv::resize(im, tmp_, cv::Size(256, im.size().height * 256 / im.size().width));
		cv::imshow(name, tmp_);
	} else cv::imshow(name, im);
#endif
}


void r3d_depth_image::WaitKey(int sec) {
#if SHOW_WINDOWS == 1
	cv::waitKey(sec);
#endif
}

